1//
2// Academic License - for use in teaching, academic research, and meeting
3// course requirements at degree granting institutions only. Not for
4// government, commercial, or other organizational use.
5//
6// File: cartesian_trajectory_planner.cpp
7//
8// Code generated for Simulink model 'cartesian_trajectory_planner'.
9//
10// Model version : 1.130
11// Simulink Coder version : 9.3 (R2020a) 18-Nov-2019
12// C/C++ source code generated on : Tue May 19 17:06:24 2020
13//
14// Target selection: ert.tlc
15// Embedded hardware selection: Generic->Unspecified (assume 32-bit Generic)
16// Code generation objectives: Unspecified
17// Validation result: Not run
18//
19#include "cartesian_trajectory_planner.h"
20#include "cartesian_trajectory_planner_private.h"
21
22// Block signals (default storage)
23B_cartesian_trajectory_planne_T cartesian_trajectory_planner_B;
24
25// Block states (default storage)
26DW_cartesian_trajectory_plann_T cartesian_trajectory_planner_DW;
27
28// Real-time model
29RT_MODEL_cartesian_trajectory_T cartesian_trajectory_planner_M_ =
30 RT_MODEL_cartesian_trajectory_T();
31RT_MODEL_cartesian_trajectory_T *const cartesian_trajectory_planner_M =
32 &cartesian_trajectory_planner_M_;
33
34// Forward declaration for local functions
35static void cart_constructLinearTimeScaling(const real_T timeInterval[2], real_T
36 time, real_T scaling[3]);
37static void carte_quaternioncg_quaternioncg(const real_T varargin_1[9], real_T
38 *obj_a, real_T *obj_b, real_T *obj_c, real_T *obj_d);
39static void cartesian__quaternionBase_slerp(real_T q1_a, real_T q1_b, real_T
40 q1_c, real_T q1_d, real_T q2_a, real_T q2_b, real_T q2_c, real_T q2_d, real_T *
41 qo_a, real_T *qo_b, real_T *qo_c, real_T *qo_d);
42static void cartesia_quaternionBase_slerp_a(real_T q1_a, real_T q1_b, real_T
43 q1_c, real_T q1_d, real_T q2_a, real_T q2_b, real_T q2_c, real_T q2_d, real_T *
44 qo_a, real_T *qo_b, real_T *qo_c, real_T *qo_d);
45static void cartesian_quaternionBase_mtimes(real_T x_a, real_T x_b, real_T x_c,
46 real_T x_d, real_T y_a, real_T y_b, real_T y_c, real_T y_d, real_T *o_a,
47 real_T *o_b, real_T *o_c, real_T *o_d);
48static void cartesian_tr_quaternionBase_log(real_T q_a, real_T q_b, real_T q_c,
49 real_T q_d, real_T *b_q_a, real_T *b_q_b, real_T *b_q_c, real_T *b_q_d);
50static void cartesian_trajectory_pl_rottraj(const real_T R0[9], const real_T RF
51 [9], const real_T varargin_2[3], real_T R[9], real_T omega[3], real_T alpha[3]);
52static void cartesian_trajec_emxInit_real_T(emxArray_real_T_cartesian_tra_T
53 **pEmxArray, int32_T numDimensions);
54static void cartes_emxEnsureCapacity_real_T(emxArray_real_T_cartesian_tra_T
55 *emxArray, int32_T oldNumel);
56static void cartesian_trajec_emxInit_char_T(emxArray_char_T_cartesian_tra_T
57 **pEmxArray, int32_T numDimensions);
58static void cartes_emxEnsureCapacity_char_T(emxArray_char_T_cartesian_tra_T
59 *emxArray, int32_T oldNumel);
60static void cartesian_trajec_emxFree_real_T(emxArray_real_T_cartesian_tra_T
61 **pEmxArray);
62static void cartesian_trajec_emxFree_char_T(emxArray_char_T_cartesian_tra_T
63 **pEmxArray);
64static void car_inverseKinematics_setupImpl(b_inverseKinematics_cartesian_T *obj,
65 f_robotics_manip_internal_IKE_T *iobj_0);
66static void c_inverseKinematics_setPoseGoal(b_inverseKinematics_cartesian_T *obj,
67 const real_T tform[16], const real_T weights[6]);
68static void RigidBodyTree_validateConfigu_a(x_robotics_manip_internal_Rig_T *obj,
69 real_T Q[6]);
70static boolean_T cartesian_trajectory_pla_strcmp(const
71 emxArray_char_T_cartesian_tra_T *a, const emxArray_char_T_cartesian_tra_T *b);
72static void rigidBodyJoint_get_JointAxis_a(const c_rigidBodyJoint_cartesian__a_T
73 *obj, real_T ax[3]);
74static void cartesian_trajectory_planne_cat(real_T varargin_1, real_T varargin_2,
75 real_T varargin_3, real_T varargin_4, real_T varargin_5, real_T varargin_6,
76 real_T varargin_7, real_T varargin_8, real_T varargin_9, real_T y[9]);
77static void rigidBodyJoint_transformBodyT_a(const
78 c_rigidBodyJoint_cartesian__a_T *obj, const real_T q_data[], const int32_T
79 *q_size, real_T T[16]);
80static void rigidBodyJoint_transformBodyToP(const
81 c_rigidBodyJoint_cartesian__a_T *obj, real_T T[16]);
82static void RigidBodyTree_efficientFKAndJac(x_robotics_manip_internal_Rig_T *obj,
83 const real_T qv[6], const emxArray_char_T_cartesian_tra_T *body1Name, real_T
84 T_data[], int32_T T_size[2], emxArray_real_T_cartesian_tra_T *Jac);
85static creal_T cartesian_trajectory_plann_sqrt(const creal_T x);
86static real_T cartesian_trajectory_plan_xnrm2(int32_T n, const real_T x[9],
87 int32_T ix0);
88static real_T cartesian_trajectory_plan_xdotc(int32_T n, const real_T x[9],
89 int32_T ix0, const real_T y[9], int32_T iy0);
90static void cartesian_trajectory_plan_xaxpy(int32_T n, real_T a, int32_T ix0,
91 const real_T y[9], int32_T iy0, real_T b_y[9]);
92static real_T cartesian_trajectory_pl_xnrm2_a(const real_T x[3], int32_T ix0);
93static void cartesian_trajectory__xaxpy_ast(int32_T n, real_T a, const real_T x
94 [9], int32_T ix0, real_T y[3], int32_T iy0);
95static void cartesian_trajectory_p_xaxpy_as(int32_T n, real_T a, const real_T x
96 [3], int32_T ix0, const real_T y[9], int32_T iy0, real_T b_y[9]);
97static void cartesian_trajectory_plan_xswap(const real_T x[9], int32_T ix0,
98 int32_T iy0, real_T b_x[9]);
99static void cartesian_trajectory_plan_xrotg(real_T a, real_T b, real_T *b_a,
100 real_T *b_b, real_T *c, real_T *s);
101static void cartesian_trajectory_plann_xrot(const real_T x[9], int32_T ix0,
102 int32_T iy0, real_T c, real_T s, real_T b_x[9]);
103static void cartesian_trajectory_planne_svd(const real_T A[9], real_T U[9],
104 real_T s[3], real_T V[9]);
105static void cartesian_trajectory_rotm2axang(const real_T R[9], real_T axang[4]);
106static void cartesian_IKHelpers_computeCost(const real_T x[6],
107 f_robotics_manip_internal_IKE_T *args, real_T *cost, real_T W[36],
108 emxArray_real_T_cartesian_tra_T *Jac, f_robotics_manip_internal_IKE_T **b_args);
109static void cartesian_trajectory_planne_eye(real_T b_I[36]);
110static void cartesian_tra_emxInit_boolean_T(emxArray_boolean_T_cartesian__T
111 **pEmxArray, int32_T numDimensions);
112static void cartesian_traje_emxInit_int32_T(emxArray_int32_T_cartesian_tr_T
113 **pEmxArray, int32_T numDimensions);
114static void car_emxEnsureCapacity_boolean_T(emxArray_boolean_T_cartesian__T
115 *emxArray, int32_T oldNumel);
116static void carte_emxEnsureCapacity_int32_T(emxArray_int32_T_cartesian_tr_T
117 *emxArray, int32_T oldNumel);
118static real_T cartesian_trajectory_pla_norm_a(const real_T x[6]);
119static real_T SystemTimeProvider_getElapsedTi(const
120 f_robotics_core_internal_Syst_T *obj);
121static real_T cartesian_trajectory_p_xnrm2_as(int32_T n, const
122 emxArray_real_T_cartesian_tra_T *x, int32_T ix0);
123static void cartesian_trajectory_pla_qrpf_a(const
124 emxArray_real_T_cartesian_tra_T *A, int32_T m, int32_T n,
125 emxArray_real_T_cartesian_tra_T *tau, const emxArray_int32_T_cartesian_tr_T
126 *jpvt, emxArray_real_T_cartesian_tra_T *b_A, emxArray_int32_T_cartesian_tr_T
127 *b_jpvt);
128static void cartesian_trajectory_pl_xzgetrf(int32_T m, int32_T n, const
129 emxArray_real_T_cartesian_tra_T *A, int32_T lda,
130 emxArray_real_T_cartesian_tra_T *b_A, emxArray_int32_T_cartesian_tr_T *ipiv,
131 int32_T *info);
132static void cartesian_trajectory_plan_xtrsm(int32_T m, int32_T n, const
133 emxArray_real_T_cartesian_tra_T *A, int32_T lda, const
134 emxArray_real_T_cartesian_tra_T *B, int32_T ldb,
135 emxArray_real_T_cartesian_tra_T *b_B);
136static void cartesian_traje_emxFree_int32_T(emxArray_int32_T_cartesian_tr_T
137 **pEmxArray);
138static void cartesian_trajectory_p_mldivide(const
139 emxArray_real_T_cartesian_tra_T *A, const emxArray_real_T_cartesian_tra_T *B,
140 emxArray_real_T_cartesian_tra_T *Y);
141static void cartesian_tra_emxFree_boolean_T(emxArray_boolean_T_cartesian__T
142 **pEmxArray);
143static boolean_T DampedBFGSwGradientProjection_a(const
144 h_robotics_core_internal_Damp_T *obj, const real_T Hg[6], const
145 emxArray_real_T_cartesian_tra_T *alpha);
146static void cartesian_trajectory_planne_inv(const
147 emxArray_real_T_cartesian_tra_T *x, emxArray_real_T_cartesian_tra_T *y);
148static void cartesian_trajectory_plann_diag(const
149 emxArray_real_T_cartesian_tra_T *v, emxArray_real_T_cartesian_tra_T *d);
150static void cartesian_trajectory_pl_sqrt_as(emxArray_real_T_cartesian_tra_T *x);
151static boolean_T cartesian_trajectory_planne_any(const
152 emxArray_boolean_T_cartesian__T *x);
153static boolean_T cartesian_tr_isPositiveDefinite(const real_T B[36]);
154static boolean_T DampedBFGSwGradientProjectio_as(const
155 h_robotics_core_internal_Damp_T *obj, const real_T xNew[6]);
156static void DampedBFGSwGradientProjection_s(h_robotics_core_internal_Damp_T *obj,
157 real_T xSol[6], c_robotics_core_internal_NLPS_T *exitFlag, real_T *err, real_T
158 *iter);
159static void cartesian_trajectory_p_isfinite(const
160 emxArray_real_T_cartesian_tra_T *x, emxArray_boolean_T_cartesian__T *b);
161static void cartesi_genrand_uint32_vector_a(uint32_T mt[625], uint32_T u[2]);
162static boolean_T cartesian_trajec_is_valid_state(const uint32_T mt[625]);
163static real_T cartesian_trajectory_genrandu_a(uint32_T mt[625]);
164static real_T cartesia_eml_rand_mt19937ar_ast(uint32_T state[625]);
165static void cartesian_trajectory_plan_randn(const real_T varargin_1[2],
166 emxArray_real_T_cartesian_tra_T *r);
167static void cartesian__eml_rand_mt19937ar_a(const uint32_T state[625], uint32_T
168 b_state[625], real_T *r);
169static void cartesian_trajectory_pla_rand_a(real_T varargin_1,
170 emxArray_real_T_cartesian_tra_T *r);
171static void cartes_NLPSolverInterface_solve(h_robotics_core_internal_Damp_T *obj,
172 const real_T seed[6], real_T xSol[6], real_T *solutionInfo_Iterations, real_T *
173 solutionInfo_RRAttempts, real_T *solutionInfo_Error, real_T
174 *solutionInfo_ExitFlag, char_T solutionInfo_Status_data[], int32_T
175 solutionInfo_Status_size[2]);
176static void cart_inverseKinematics_stepImpl(b_inverseKinematics_cartesian_T *obj,
177 const real_T tform[16], const real_T weights[6], const real_T initialGuess[6],
178 real_T QSol[6]);
179static void cartesian_t_emxInit_f_cell_wrap(emxArray_f_cell_wrap_cartesia_T
180 **pEmxArray, int32_T numDimensions);
181static void c_emxEnsureCapacity_f_cell_wrap(emxArray_f_cell_wrap_cartesia_T
182 *emxArray, int32_T oldNumel);
183static void ca_rigidBodyJoint_get_JointAxis(const
184 c_rigidBodyJoint_cartesian_tr_T *obj, real_T ax[3]);
185static void RigidBodyTree_forwardKinematics(p_robotics_manip_internal_Rig_T *obj,
186 const real_T qvec[6], emxArray_f_cell_wrap_cartesia_T *Ttree);
187static void cartesian_t_emxFree_f_cell_wrap(emxArray_f_cell_wrap_cartesia_T
188 **pEmxArray);
189static void RigidBodyTree_geometricJacobian(p_robotics_manip_internal_Rig_T *obj,
190 const real_T Q[6], emxArray_real_T_cartesian_tra_T *Jac);
191static void matlabCodegenHandle_matlabC_ast(ros_slros_internal_block_Subs_T *obj);
192static void matlabCodegenHandle_matlab_astw(ros_slros_internal_block_GetP_T *obj);
193static void matlabCodegenHandle_matlabCod_a(robotics_slmanip_internal_b_a_T *obj);
194static void cartesian_tr_SystemCore_release(b_inverseKinematics_cartesian_T *obj);
195static void cartesian_tra_SystemCore_delete(b_inverseKinematics_cartesian_T *obj);
196static void matlabCodegenHandle_matlabCodeg(b_inverseKinematics_cartesian_T *obj);
197static void emxFreeStruct_c_rigidBodyJoint(c_rigidBodyJoint_cartesian__a_T
198 *pStruct);
199static void emxFreeStruct_v_robotics_manip_(v_robotics_manip_internal_Rig_T
200 *pStruct);
201static void emxFreeStruct_y_robotics_manip_(y_robotics_manip_internal_Rig_T
202 *pStruct);
203static void emxFreeStruct_b_inverseKinemati(b_inverseKinematics_cartesian_T
204 *pStruct);
205static void emxFreeStruct_robotics_slmanip_(robotics_slmanip_internal_b_a_T
206 *pStruct);
207static void emxFreeStruct_w_robotics_manip_(w_robotics_manip_internal_Rig_T
208 *pStruct);
209static void emxFreeStruct_x_robotics_manip_(x_robotics_manip_internal_Rig_T
210 *pStruct);
211static void emxFreeStruct_f_robotics_manip_(f_robotics_manip_internal_IKE_T
212 *pStruct);
213static void emxFreeStruct_h_robotics_core_i(h_robotics_core_internal_Damp_T
214 *pStruct);
215static void emxFreeStruct_c_rigidBodyJoint1(c_rigidBodyJoint_cartesian_tr_T
216 *pStruct);
217static void emxFreeStruct_o_robotics_manip_(o_robotics_manip_internal_Rig_T
218 *pStruct);
219static void emxFreeStruct_p_robotics_manip_(p_robotics_manip_internal_Rig_T
220 *pStruct);
221static void emxFreeStruct_robotics_slmani_a(robotics_slmanip_internal_blo_T
222 *pStruct);
223static void emxFreeStruct_n_robotics_manip_(n_robotics_manip_internal_Rig_T
224 *pStruct);
225static void matlabCodegenHandle_matlabCo_as(ros_slros_internal_block_Publ_T *obj);
226static void cartesian_traj_SystemCore_setup(robotics_slcore_internal_bl_a_T *obj);
227static void emxInitStruct_c_rigidBodyJoint(c_rigidBodyJoint_cartesian__a_T
228 *pStruct);
229static void emxInitStruct_v_robotics_manip_(v_robotics_manip_internal_Rig_T
230 *pStruct);
231static void emxInitStruct_y_robotics_manip_(y_robotics_manip_internal_Rig_T
232 *pStruct);
233static void emxInitStruct_b_inverseKinemati(b_inverseKinematics_cartesian_T
234 *pStruct);
235static void emxInitStruct_robotics_slmanip_(robotics_slmanip_internal_b_a_T
236 *pStruct);
237static void emxInitStruct_w_robotics_manip_(w_robotics_manip_internal_Rig_T
238 *pStruct);
239static void emxInitStruct_x_robotics_manip_(x_robotics_manip_internal_Rig_T
240 *pStruct);
241static void emxInitStruct_f_robotics_manip_(f_robotics_manip_internal_IKE_T
242 *pStruct);
243static void emxInitStruct_h_robotics_core_i(h_robotics_core_internal_Damp_T
244 *pStruct);
245static void cartesia_twister_state_vector_a(uint32_T mt[625]);
246static void cartesian_tr_eml_rand_mt19937ar(uint32_T state[625]);
247static v_robotics_manip_internal_Rig_T *c_RigidBody_RigidBody_astwhqf2a
248 (v_robotics_manip_internal_Rig_T *obj);
249static v_robotics_manip_internal_Rig_T *RigidBody_RigidBody_astwhqf2az
250 (v_robotics_manip_internal_Rig_T *obj);
251static v_robotics_manip_internal_Rig_T *RigidBody_RigidBody_astwhqf2azt
252 (v_robotics_manip_internal_Rig_T *obj);
253static v_robotics_manip_internal_Rig_T *RigidBody_RigidBod_astwhqf2aztt
254 (v_robotics_manip_internal_Rig_T *obj);
255static v_robotics_manip_internal_Rig_T *RigidBody_RigidBo_astwhqf2azttx
256 (v_robotics_manip_internal_Rig_T *obj);
257static v_robotics_manip_internal_Rig_T *RigidBody_RigidB_astwhqf2azttxa
258 (v_robotics_manip_internal_Rig_T *obj);
259static v_robotics_manip_internal_Rig_T *RigidBody_Rigid_astwhqf2azttxab
260 (v_robotics_manip_internal_Rig_T *obj);
261static y_robotics_manip_internal_Rig_T *c_RigidBodyTree_RigidBodyTree_a
262 (y_robotics_manip_internal_Rig_T *obj, v_robotics_manip_internal_Rig_T *iobj_0,
263 v_robotics_manip_internal_Rig_T *iobj_1, v_robotics_manip_internal_Rig_T
264 *iobj_2, v_robotics_manip_internal_Rig_T *iobj_3,
265 v_robotics_manip_internal_Rig_T *iobj_4, v_robotics_manip_internal_Rig_T
266 *iobj_5, v_robotics_manip_internal_Rig_T *iobj_6,
267 v_robotics_manip_internal_Rig_T *iobj_7);
268static void cartesian_trajectory_plann_rand(real_T r[5]);
269static w_robotics_manip_internal_Rig_T *c_RigidBody_Rigid_b
270 (w_robotics_manip_internal_Rig_T *obj, c_rigidBodyJoint_cartesian__a_T *iobj_0);
271static w_robotics_manip_internal_Rig_T *c_RigidBody_Rigid_i
272 (w_robotics_manip_internal_Rig_T *obj, c_rigidBodyJoint_cartesian__a_T *iobj_0);
273static w_robotics_manip_internal_Rig_T *c_RigidBody_Rigid_l
274 (w_robotics_manip_internal_Rig_T *obj, c_rigidBodyJoint_cartesian__a_T *iobj_0);
275static w_robotics_manip_internal_Rig_T *c_RigidBody_Rigid_lh
276 (w_robotics_manip_internal_Rig_T *obj, c_rigidBodyJoint_cartesian__a_T *iobj_0);
277static w_robotics_manip_internal_Rig_T *c_RigidBody_Rigid_k
278 (w_robotics_manip_internal_Rig_T *obj, c_rigidBodyJoint_cartesian__a_T *iobj_0);
279static void ca_RigidBodyTree_clearAllBodies(x_robotics_manip_internal_Rig_T *obj,
280 w_robotics_manip_internal_Rig_T *iobj_0, w_robotics_manip_internal_Rig_T
281 *iobj_1, w_robotics_manip_internal_Rig_T *iobj_2,
282 w_robotics_manip_internal_Rig_T *iobj_3, w_robotics_manip_internal_Rig_T
283 *iobj_4, w_robotics_manip_internal_Rig_T *iobj_5,
284 w_robotics_manip_internal_Rig_T *iobj_6, c_rigidBodyJoint_cartesian__a_T
285 *iobj_7, c_rigidBodyJoint_cartesian__a_T *iobj_8,
286 c_rigidBodyJoint_cartesian__a_T *iobj_9, c_rigidBodyJoint_cartesian__a_T
287 *iobj_10, c_rigidBodyJoint_cartesian__a_T *iobj_11,
288 c_rigidBodyJoint_cartesian__a_T *iobj_12, c_rigidBodyJoint_cartesian__a_T
289 *iobj_13, c_rigidBodyJoint_cartesian__a_T *iobj_14,
290 w_robotics_manip_internal_Rig_T *iobj_15);
291static x_robotics_manip_internal_Rig_T *RigidBodyTree_RigidBodyTree_as
292 (x_robotics_manip_internal_Rig_T *obj, w_robotics_manip_internal_Rig_T *iobj_0,
293 w_robotics_manip_internal_Rig_T *iobj_1, w_robotics_manip_internal_Rig_T
294 *iobj_2, w_robotics_manip_internal_Rig_T *iobj_3,
295 w_robotics_manip_internal_Rig_T *iobj_4, w_robotics_manip_internal_Rig_T
296 *iobj_5, w_robotics_manip_internal_Rig_T *iobj_6,
297 c_rigidBodyJoint_cartesian__a_T *iobj_7, c_rigidBodyJoint_cartesian__a_T
298 *iobj_8, c_rigidBodyJoint_cartesian__a_T *iobj_9,
299 c_rigidBodyJoint_cartesian__a_T *iobj_10, c_rigidBodyJoint_cartesian__a_T
300 *iobj_11, c_rigidBodyJoint_cartesian__a_T *iobj_12,
301 c_rigidBodyJoint_cartesian__a_T *iobj_13, c_rigidBodyJoint_cartesian__a_T
302 *iobj_14, c_rigidBodyJoint_cartesian__a_T *iobj_15,
303 w_robotics_manip_internal_Rig_T *iobj_16);
304static c_rigidBodyJoint_cartesian__a_T *c_rigidBodyJoint_rigidBodyJoint
305 (c_rigidBodyJoint_cartesian__a_T *obj, const emxArray_char_T_cartesian_tra_T
306 *jname, const emxArray_char_T_cartesian_tra_T *jtype);
307static w_robotics_manip_internal_Rig_T *cartesian_trajec_RigidBody_copy(const
308 v_robotics_manip_internal_Rig_T *obj, c_rigidBodyJoint_cartesian__a_T *iobj_0,
309 c_rigidBodyJoint_cartesian__a_T *iobj_1, w_robotics_manip_internal_Rig_T
310 *iobj_2);
311static void cartesian_RigidBodyTree_addBody(x_robotics_manip_internal_Rig_T *obj,
312 v_robotics_manip_internal_Rig_T *bodyin, const emxArray_char_T_cartesian_tra_T
313 *parentName, c_rigidBodyJoint_cartesian__a_T *iobj_0,
314 c_rigidBodyJoint_cartesian__a_T *iobj_1, w_robotics_manip_internal_Rig_T
315 *iobj_2);
316static void inverseKinematics_set_RigidBody(b_inverseKinematics_cartesian_T *obj,
317 y_robotics_manip_internal_Rig_T *rigidbodytree,
318 w_robotics_manip_internal_Rig_T *iobj_0, w_robotics_manip_internal_Rig_T
319 *iobj_1, w_robotics_manip_internal_Rig_T *iobj_2,
320 w_robotics_manip_internal_Rig_T *iobj_3, w_robotics_manip_internal_Rig_T
321 *iobj_4, w_robotics_manip_internal_Rig_T *iobj_5,
322 w_robotics_manip_internal_Rig_T *iobj_6, w_robotics_manip_internal_Rig_T
323 *iobj_7, w_robotics_manip_internal_Rig_T *iobj_8,
324 w_robotics_manip_internal_Rig_T *iobj_9, w_robotics_manip_internal_Rig_T
325 *iobj_10, w_robotics_manip_internal_Rig_T *iobj_11,
326 w_robotics_manip_internal_Rig_T *iobj_12, w_robotics_manip_internal_Rig_T
327 *iobj_13, w_robotics_manip_internal_Rig_T *iobj_14,
328 c_rigidBodyJoint_cartesian__a_T *iobj_15, c_rigidBodyJoint_cartesian__a_T
329 *iobj_16, c_rigidBodyJoint_cartesian__a_T *iobj_17,
330 c_rigidBodyJoint_cartesian__a_T *iobj_18, c_rigidBodyJoint_cartesian__a_T
331 *iobj_19, c_rigidBodyJoint_cartesian__a_T *iobj_20,
332 c_rigidBodyJoint_cartesian__a_T *iobj_21, c_rigidBodyJoint_cartesian__a_T
333 *iobj_22, c_rigidBodyJoint_cartesian__a_T *iobj_23,
334 c_rigidBodyJoint_cartesian__a_T *iobj_24, c_rigidBodyJoint_cartesian__a_T
335 *iobj_25, c_rigidBodyJoint_cartesian__a_T *iobj_26,
336 c_rigidBodyJoint_cartesian__a_T *iobj_27, c_rigidBodyJoint_cartesian__a_T
337 *iobj_28, c_rigidBodyJoint_cartesian__a_T *iobj_29,
338 c_rigidBodyJoint_cartesian__a_T *iobj_30, c_rigidBodyJoint_cartesian__a_T
339 *iobj_31, c_rigidBodyJoint_cartesian__a_T *iobj_32,
340 c_rigidBodyJoint_cartesian__a_T *iobj_33, c_rigidBodyJoint_cartesian__a_T
341 *iobj_34, c_rigidBodyJoint_cartesian__a_T *iobj_35,
342 c_rigidBodyJoint_cartesian__a_T *iobj_36, c_rigidBodyJoint_cartesian__a_T
343 *iobj_37, c_rigidBodyJoint_cartesian__a_T *iobj_38,
344 c_rigidBodyJoint_cartesian__a_T *iobj_39, w_robotics_manip_internal_Rig_T
345 *iobj_40, x_robotics_manip_internal_Rig_T *iobj_41);
346static h_robotics_core_internal_Damp_T *DampedBFGSwGradientProjection_D
347 (h_robotics_core_internal_Damp_T *obj);
348static void emxInitStruct_c_rigidBodyJoint1(c_rigidBodyJoint_cartesian_tr_T
349 *pStruct);
350static void emxInitStruct_o_robotics_manip_(o_robotics_manip_internal_Rig_T
351 *pStruct);
352static void emxInitStruct_p_robotics_manip_(p_robotics_manip_internal_Rig_T
353 *pStruct);
354static void emxInitStruct_robotics_slmani_a(robotics_slmanip_internal_blo_T
355 *pStruct);
356static void emxInitStruct_n_robotics_manip_(n_robotics_manip_internal_Rig_T
357 *pStruct);
358static n_robotics_manip_internal_Rig_T *cartesian_t_RigidBody_RigidBody
359 (n_robotics_manip_internal_Rig_T *obj);
360static n_robotics_manip_internal_Rig_T *cartesian_RigidBody_RigidBody_a
361 (n_robotics_manip_internal_Rig_T *obj);
362static n_robotics_manip_internal_Rig_T *cartesia_RigidBody_RigidBody_as
363 (n_robotics_manip_internal_Rig_T *obj);
364static n_robotics_manip_internal_Rig_T *cartesi_RigidBody_RigidBody_ast
365 (n_robotics_manip_internal_Rig_T *obj);
366static n_robotics_manip_internal_Rig_T *cartes_RigidBody_RigidBody_astw
367 (n_robotics_manip_internal_Rig_T *obj);
368static n_robotics_manip_internal_Rig_T *carte_RigidBody_RigidBody_astwh
369 (n_robotics_manip_internal_Rig_T *obj);
370static n_robotics_manip_internal_Rig_T *cart_RigidBody_RigidBody_astwhq
371 (n_robotics_manip_internal_Rig_T *obj);
372static n_robotics_manip_internal_Rig_T *car_RigidBody_RigidBody_astwhqf
373 (n_robotics_manip_internal_Rig_T *obj);
374static o_robotics_manip_internal_Rig_T *ca_RigidBody_RigidBody_astwhqf2
375 (o_robotics_manip_internal_Rig_T *obj);
376static p_robotics_manip_internal_Rig_T *car_RigidBodyTree_RigidBodyTree
377 (p_robotics_manip_internal_Rig_T *obj, n_robotics_manip_internal_Rig_T *iobj_0,
378 n_robotics_manip_internal_Rig_T *iobj_1, n_robotics_manip_internal_Rig_T
379 *iobj_2, n_robotics_manip_internal_Rig_T *iobj_3,
380 n_robotics_manip_internal_Rig_T *iobj_4, n_robotics_manip_internal_Rig_T
381 *iobj_5, n_robotics_manip_internal_Rig_T *iobj_6,
382 n_robotics_manip_internal_Rig_T *iobj_7);
383int32_T div_s32_floor(int32_T numerator, int32_T denominator)
384{
385 int32_T quotient;
386 uint32_T absNumerator;
387 uint32_T absDenominator;
388 uint32_T tempAbsQuotient;
389 boolean_T quotientNeedsNegation;
390 if (denominator == 0) {
391 quotient = numerator >= 0 ? MAX_int32_T : MIN_int32_T;
392
393 // Divide by zero handler
394 } else {
395 absNumerator = numerator < 0 ? ~static_cast<uint32_T>(numerator) + 1U :
396 static_cast<uint32_T>(numerator);
397 absDenominator = denominator < 0 ? ~static_cast<uint32_T>(denominator) + 1U :
398 static_cast<uint32_T>(denominator);
399 quotientNeedsNegation = ((numerator < 0) != (denominator < 0));
400 tempAbsQuotient = absNumerator / absDenominator;
401 if (quotientNeedsNegation) {
402 absNumerator %= absDenominator;
403 if (absNumerator > 0U) {
404 tempAbsQuotient++;
405 }
406 }
407
408 quotient = quotientNeedsNegation ? -static_cast<int32_T>(tempAbsQuotient) :
409 static_cast<int32_T>(tempAbsQuotient);
410 }
411
412 return quotient;
413}
414
415static void cart_constructLinearTimeScaling(const real_T timeInterval[2], real_T
416 time, real_T scaling[3])
417{
418 real_T linearScaling;
419 real_T b;
420 linearScaling = 1.0 / (timeInterval[1] - timeInterval[0]);
421 b = (time - timeInterval[0]) * linearScaling;
422 if (time < timeInterval[0]) {
423 b = 0.0;
424 }
425
426 if (time > timeInterval[1]) {
427 b = 1.0;
428 }
429
430 if (time < timeInterval[0]) {
431 linearScaling = 0.0;
432 }
433
434 if (time > timeInterval[1]) {
435 linearScaling = 0.0;
436 }
437
438 scaling[0] = b;
439 scaling[1] = linearScaling;
440 scaling[2] = 0.0;
441}
442
443static void carte_quaternioncg_quaternioncg(const real_T varargin_1[9], real_T
444 *obj_a, real_T *obj_b, real_T *obj_c, real_T *obj_d)
445{
446 real_T b[9];
447 real_T psquared[4];
448 real_T pd;
449 int32_T b_k;
450 int32_T b_idx;
451 int32_T subsa_idx_1;
452 real_T tmp;
453 boolean_T exitg1;
454 for (b_k = 0; b_k < 3; b_k++) {
455 subsa_idx_1 = b_k + 1;
456 b[subsa_idx_1 - 1] = varargin_1[(subsa_idx_1 - 1) * 3];
457 subsa_idx_1 = b_k + 1;
458 b[subsa_idx_1 + 2] = varargin_1[(subsa_idx_1 - 1) * 3 + 1];
459 subsa_idx_1 = b_k + 1;
460 b[subsa_idx_1 + 5] = varargin_1[(subsa_idx_1 - 1) * 3 + 2];
461 }
462
463 pd = (b[0] + b[4]) + b[8];
464 psquared[0] = (pd * 2.0 + 1.0) - pd;
465 psquared[1] = (2.0 * b[0] + 1.0) - pd;
466 psquared[2] = (2.0 * b[4] + 1.0) - pd;
467 psquared[3] = (2.0 * b[8] + 1.0) - pd;
468 if (!rtIsNaN(psquared[0])) {
469 b_k = 1;
470 } else {
471 b_k = 0;
472 subsa_idx_1 = 2;
473 exitg1 = false;
474 while ((!exitg1) && (subsa_idx_1 < 5)) {
475 if (!rtIsNaN(psquared[subsa_idx_1 - 1])) {
476 b_k = subsa_idx_1;
477 exitg1 = true;
478 } else {
479 subsa_idx_1++;
480 }
481 }
482 }
483
484 if (b_k == 0) {
485 pd = psquared[0];
486 b_k = 1;
487 } else {
488 pd = psquared[b_k - 1];
489 b_idx = b_k;
490 for (subsa_idx_1 = b_k + 1; subsa_idx_1 < 5; subsa_idx_1++) {
491 tmp = psquared[subsa_idx_1 - 1];
492 if (pd < tmp) {
493 pd = tmp;
494 b_idx = subsa_idx_1;
495 }
496 }
497
498 b_k = b_idx;
499 }
500
501 switch (b_k) {
502 case 1:
503 pd = sqrt(pd);
504 *obj_a = 0.5 * pd;
505 pd = 0.5 / pd;
506 *obj_b = (b[7] - b[5]) * pd;
507 *obj_c = (b[2] - b[6]) * pd;
508 *obj_d = (b[3] - b[1]) * pd;
509 break;
510
511 case 2:
512 pd = sqrt(pd);
513 *obj_b = 0.5 * pd;
514 pd = 0.5 / pd;
515 *obj_a = (b[7] - b[5]) * pd;
516 *obj_c = (b[3] + b[1]) * pd;
517 *obj_d = (b[2] + b[6]) * pd;
518 break;
519
520 case 3:
521 pd = sqrt(pd);
522 *obj_c = 0.5 * pd;
523 pd = 0.5 / pd;
524 *obj_a = (b[2] - b[6]) * pd;
525 *obj_b = (b[3] + b[1]) * pd;
526 *obj_d = (b[7] + b[5]) * pd;
527 break;
528
529 default:
530 pd = sqrt(pd);
531 *obj_d = 0.5 * pd;
532 pd = 0.5 / pd;
533 *obj_a = (b[3] - b[1]) * pd;
534 *obj_b = (b[2] + b[6]) * pd;
535 *obj_c = (b[7] + b[5]) * pd;
536 break;
537 }
538
539 if (*obj_a < 0.0) {
540 *obj_a = -*obj_a;
541 *obj_b = -*obj_b;
542 *obj_c = -*obj_c;
543 *obj_d = -*obj_d;
544 }
545}
546
547static void cartesian__quaternionBase_slerp(real_T q1_a, real_T q1_b, real_T
548 q1_c, real_T q1_d, real_T q2_a, real_T q2_b, real_T q2_c, real_T q2_d, real_T *
549 qo_a, real_T *qo_b, real_T *qo_c, real_T *qo_d)
550{
551 real_T q1n_a;
552 real_T q1n_b;
553 real_T q1n_c;
554 real_T q1n_d;
555 real_T q2n_a;
556 real_T q2n_b;
557 real_T q2n_c;
558 real_T dp;
559 real_T theta0;
560 real_T n;
561 real_T sinv_tmp;
562 n = sqrt(((q1_a * q1_a + q1_b * q1_b) + q1_c * q1_c) + q1_d * q1_d);
563 q1n_a = q1_a / n;
564 q1n_b = q1_b / n;
565 q1n_c = q1_c / n;
566 q1n_d = q1_d / n;
567 n = sqrt(((q2_a * q2_a + q2_b * q2_b) + q2_c * q2_c) + q2_d * q2_d);
568 q2n_a = q2_a / n;
569 q2n_b = q2_b / n;
570 q2n_c = q2_c / n;
571 n = q2_d / n;
572 dp = ((q1n_a * q2n_a + q1n_b * q2n_b) + q1n_c * q2n_c) + q1n_d * n;
573 if (dp < 0.0) {
574 q2n_a = -q2n_a;
575 q2n_b = -q2n_b;
576 q2n_c = -q2n_c;
577 n = -n;
578 dp = -dp;
579 }
580
581 if (dp > 1.0) {
582 dp = 1.0;
583 }
584
585 theta0 = acos(dp);
586 sinv_tmp = sin(theta0);
587 dp = 1.0 / sinv_tmp;
588 theta0 = sin(0.0 * theta0);
589 *qo_a = (sinv_tmp * q1n_a + theta0 * q2n_a) * dp;
590 *qo_b = (sinv_tmp * q1n_b + theta0 * q2n_b) * dp;
591 *qo_c = (sinv_tmp * q1n_c + theta0 * q2n_c) * dp;
592 *qo_d = (sinv_tmp * q1n_d + theta0 * n) * dp;
593 if (rtIsInf(dp)) {
594 *qo_a = q1n_a;
595 *qo_b = q1n_b;
596 *qo_c = q1n_c;
597 *qo_d = q1n_d;
598 }
599
600 n = sqrt(((*qo_a * *qo_a + *qo_b * *qo_b) + *qo_c * *qo_c) + *qo_d * *qo_d);
601 *qo_a /= n;
602 *qo_b /= n;
603 *qo_c /= n;
604 *qo_d /= n;
605}
606
607static void cartesia_quaternionBase_slerp_a(real_T q1_a, real_T q1_b, real_T
608 q1_c, real_T q1_d, real_T q2_a, real_T q2_b, real_T q2_c, real_T q2_d, real_T *
609 qo_a, real_T *qo_b, real_T *qo_c, real_T *qo_d)
610{
611 real_T q1n_a;
612 real_T q1n_b;
613 real_T q1n_c;
614 real_T q1n_d;
615 real_T q2n_a;
616 real_T q2n_b;
617 real_T q2n_c;
618 real_T dp;
619 real_T theta0;
620 real_T n;
621 real_T sinv_tmp;
622 n = sqrt(((q1_a * q1_a + q1_b * q1_b) + q1_c * q1_c) + q1_d * q1_d);
623 q1n_a = q1_a / n;
624 q1n_b = q1_b / n;
625 q1n_c = q1_c / n;
626 q1n_d = q1_d / n;
627 n = sqrt(((q2_a * q2_a + q2_b * q2_b) + q2_c * q2_c) + q2_d * q2_d);
628 q2n_a = q2_a / n;
629 q2n_b = q2_b / n;
630 q2n_c = q2_c / n;
631 n = q2_d / n;
632 dp = ((q1n_a * q2n_a + q1n_b * q2n_b) + q1n_c * q2n_c) + q1n_d * n;
633 if (dp < 0.0) {
634 q2n_a = -q2n_a;
635 q2n_b = -q2n_b;
636 q2n_c = -q2n_c;
637 n = -n;
638 dp = -dp;
639 }
640
641 if (dp > 1.0) {
642 dp = 1.0;
643 }
644
645 theta0 = acos(dp);
646 sinv_tmp = sin(theta0);
647 dp = 1.0 / sinv_tmp;
648 theta0 = sin(0.0 * theta0);
649 *qo_a = (theta0 * q1n_a + sinv_tmp * q2n_a) * dp;
650 *qo_b = (theta0 * q1n_b + sinv_tmp * q2n_b) * dp;
651 *qo_c = (theta0 * q1n_c + sinv_tmp * q2n_c) * dp;
652 *qo_d = (theta0 * q1n_d + sinv_tmp * n) * dp;
653 if (rtIsInf(dp)) {
654 *qo_a = q1n_a;
655 *qo_b = q1n_b;
656 *qo_c = q1n_c;
657 *qo_d = q1n_d;
658 }
659
660 n = sqrt(((*qo_a * *qo_a + *qo_b * *qo_b) + *qo_c * *qo_c) + *qo_d * *qo_d);
661 *qo_a /= n;
662 *qo_b /= n;
663 *qo_c /= n;
664 *qo_d /= n;
665}
666
667static void cartesian_quaternionBase_mtimes(real_T x_a, real_T x_b, real_T x_c,
668 real_T x_d, real_T y_a, real_T y_b, real_T y_c, real_T y_d, real_T *o_a,
669 real_T *o_b, real_T *o_c, real_T *o_d)
670{
671 *o_a = ((x_a * y_a - x_b * y_b) - x_c * y_c) - x_d * y_d;
672 *o_b = ((x_a * y_b + x_b * y_a) + x_c * y_d) - x_d * y_c;
673 *o_c = ((x_a * y_c - x_b * y_d) + x_c * y_a) + x_d * y_b;
674 *o_d = ((x_a * y_d + x_b * y_c) - x_c * y_b) + x_d * y_a;
675}
676
677static void cartesian_tr_quaternionBase_log(real_T q_a, real_T q_b, real_T q_c,
678 real_T q_d, real_T *b_q_a, real_T *b_q_b, real_T *b_q_c, real_T *b_q_d)
679{
680 real_T vnorm;
681 real_T qnorm;
682 real_T vscale_data;
683 real_T g;
684 real_T h;
685 int32_T b_trueCount;
686 real_T b_x_data;
687 int32_T nx;
688 int32_T k;
689 vnorm = sqrt((q_b * q_b + q_c * q_c) + q_d * q_d);
690 qnorm = sqrt(q_a * q_a + vnorm * vnorm);
691 b_trueCount = 0;
692 if (vnorm != 0.0) {
693 for (nx = 0; nx < 1; nx++) {
694 b_trueCount++;
695 }
696 }
697
698 nx = b_trueCount - 1;
699 for (k = 0; k <= nx; k++) {
700 vscale_data = q_a / qnorm;
701 }
702
703 nx = b_trueCount - 1;
704 if (0 <= nx) {
705 memcpy(&b_x_data, &vscale_data, (nx + 1) * sizeof(real_T));
706 }
707
708 nx = b_trueCount - 1;
709 for (k = 0; k <= nx; k++) {
710 b_x_data = acos(b_x_data);
711 }
712
713 nx = b_trueCount - 1;
714 for (k = 0; k <= nx; k++) {
715 vscale_data = b_x_data / vnorm;
716 }
717
718 nx = b_trueCount - 1;
719 for (k = 0; k <= nx; k++) {
720 b_x_data = q_b * vscale_data;
721 }
722
723 g = q_b;
724 if (vnorm != 0.0) {
725 for (k = 0; k < 1; k++) {
726 g = b_x_data;
727 }
728 }
729
730 h = g;
731 if (!(vnorm != 0.0)) {
732 h = 0.0;
733 }
734
735 nx = b_trueCount - 1;
736 for (k = 0; k <= nx; k++) {
737 b_x_data = q_c * vscale_data;
738 }
739
740 g = q_c;
741 if (vnorm != 0.0) {
742 for (k = 0; k < 1; k++) {
743 g = b_x_data;
744 }
745 }
746
747 if (!(vnorm != 0.0)) {
748 g = 0.0;
749 }
750
751 nx = b_trueCount - 1;
752 for (k = 0; k <= nx; k++) {
753 b_x_data = q_d * vscale_data;
754 }
755
756 vscale_data = q_d;
757 if (vnorm != 0.0) {
758 for (nx = 0; nx < 1; nx++) {
759 vscale_data = b_x_data;
760 }
761 }
762
763 if (!(vnorm != 0.0)) {
764 vscale_data = 0.0;
765 }
766
767 *b_q_a = log(qnorm);
768 *b_q_b = h;
769 *b_q_c = g;
770 *b_q_d = vscale_data;
771}
772
773static void cartesian_trajectory_pl_rottraj(const real_T R0[9], const real_T RF
774 [9], const real_T varargin_2[3], real_T R[9], real_T omega[3], real_T alpha[3])
775{
776 real_T pn_a;
777 real_T pn_b;
778 real_T pn_c;
779 real_T pn_d;
780 real_T qn_c;
781 real_T qn_d;
782 real_T pnCorrected_a;
783 real_T pnCorrected_b;
784 real_T pnCorrected_c;
785 real_T n;
786 real_T ab2;
787 real_T ac2;
788 real_T ad2;
789 real_T bc2;
790 real_T bd2;
791 real_T cd2;
792 real_T aasq;
793 real_T rmat[9];
794 real_T theta0;
795 real_T b_x;
796 real_T assign_temp_d;
797 real_T assign_temp_a;
798 real_T assign_temp_b;
799 real_T assign_temp_c;
800 real_T assign_temp_d_0;
801 real_T q_a[4];
802 int32_T b_k;
803 int32_T subsa_idx_1;
804 carte_quaternioncg_quaternioncg(R0, &pn_a, &pn_b, &pn_c, &pn_d);
805 n = sqrt(((pn_a * pn_a + pn_b * pn_b) + pn_c * pn_c) + pn_d * pn_d);
806 pn_a /= n;
807 pn_b /= n;
808 pn_c /= n;
809 pn_d /= n;
810 carte_quaternioncg_quaternioncg(RF, &cd2, &aasq, &qn_c, &qn_d);
811 n = sqrt(((cd2 * cd2 + aasq * aasq) + qn_c * qn_c) + qn_d * qn_d);
812 cd2 /= n;
813 aasq /= n;
814 qn_c /= n;
815 qn_d /= n;
816 cartesian__quaternionBase_slerp(pn_a, pn_b, pn_c, pn_d, cd2, aasq, qn_c, qn_d,
817 &pnCorrected_a, &pnCorrected_b, &pnCorrected_c, &ab2);
818 cartesia_quaternionBase_slerp_a(pn_a, pn_b, pn_c, pn_d, cd2, aasq, qn_c, qn_d,
819 &ac2, &ad2, &bc2, &bd2);
820 n = sqrt(((pn_a * pn_a + pn_b * pn_b) + pn_c * pn_c) + pn_d * pn_d);
821 pn_a /= n;
822 pn_b /= n;
823 pn_c /= n;
824 pn_d /= n;
825 n = sqrt(((cd2 * cd2 + aasq * aasq) + qn_c * qn_c) + qn_d * qn_d);
826 cd2 /= n;
827 aasq /= n;
828 qn_c /= n;
829 qn_d /= n;
830 n = ((pn_a * cd2 + pn_b * aasq) + pn_c * qn_c) + pn_d * qn_d;
831 if (n < 0.0) {
832 cd2 = -cd2;
833 aasq = -aasq;
834 qn_c = -qn_c;
835 qn_d = -qn_d;
836 n = -n;
837 }
838
839 if (n > 1.0) {
840 n = 1.0;
841 }
842
843 theta0 = acos(n);
844 n = 1.0 / sin(theta0);
845 b_x = sin((1.0 - varargin_2[0]) * theta0);
846 theta0 = sin(varargin_2[0] * theta0);
847 cd2 = (b_x * pn_a + theta0 * cd2) * n;
848 aasq = (b_x * pn_b + theta0 * aasq) * n;
849 qn_c = (b_x * pn_c + theta0 * qn_c) * n;
850 qn_d = (b_x * pn_d + theta0 * qn_d) * n;
851 if (rtIsInf(n)) {
852 cd2 = pn_a;
853 aasq = pn_b;
854 qn_c = pn_c;
855 qn_d = pn_d;
856 }
857
858 n = sqrt(((cd2 * cd2 + aasq * aasq) + qn_c * qn_c) + qn_d * qn_d);
859 cd2 /= n;
860 aasq /= n;
861 qn_c /= n;
862 qn_d /= n;
863 cartesian_quaternionBase_mtimes(pnCorrected_a, -pnCorrected_b, -pnCorrected_c,
864 -ab2, ac2, ad2, bc2, bd2, &pn_d, &b_x, &theta0, &assign_temp_d);
865 cartesian_tr_quaternionBase_log(pn_d, b_x, theta0, assign_temp_d,
866 &assign_temp_a, &assign_temp_b, &assign_temp_c, &assign_temp_d_0);
867 cartesian_quaternionBase_mtimes(cd2, aasq, qn_c, qn_d, assign_temp_a,
868 assign_temp_b, assign_temp_c, assign_temp_d_0, &pn_a, &pn_b, &n, &pn_c);
869 cartesian_quaternionBase_mtimes(2.0 * (varargin_2[1] * pn_a), 2.0 *
870 (varargin_2[1] * pn_b), 2.0 * (varargin_2[1] * n), 2.0 * (varargin_2[1] *
871 pn_c), cd2, -aasq, -qn_c, -qn_d, &pn_d, &omega[0], &omega[1], &omega[2]);
872 cartesian_quaternionBase_mtimes(pnCorrected_a, -pnCorrected_b, -pnCorrected_c,
873 -ab2, ac2, ad2, bc2, bd2, &pn_d, &b_x, &theta0, &assign_temp_d);
874 cartesian_tr_quaternionBase_log(pn_d, b_x, theta0, assign_temp_d,
875 &assign_temp_a, &assign_temp_b, &assign_temp_c, &assign_temp_d_0);
876 cartesian_quaternionBase_mtimes(cd2, aasq, qn_c, qn_d, assign_temp_a,
877 assign_temp_b, assign_temp_c, assign_temp_d_0, &pn_d, &b_x, &theta0,
878 &assign_temp_d);
879 cartesian_quaternionBase_mtimes(pnCorrected_a, -pnCorrected_b, -pnCorrected_c,
880 -ab2, ac2, ad2, bc2, bd2, &assign_temp_a, &assign_temp_b, &assign_temp_c,
881 &assign_temp_d_0);
882 cartesian_tr_quaternionBase_log(assign_temp_a, assign_temp_b, assign_temp_c,
883 assign_temp_d_0, &pnCorrected_a, &pnCorrected_b, &pnCorrected_c, &ab2);
884 cartesian_quaternionBase_mtimes(pn_d, b_x, theta0, assign_temp_d,
885 pnCorrected_a, pnCorrected_b, pnCorrected_c, ab2, &pn_a, &pn_b, &n, &pn_c);
886 cartesian_quaternionBase_mtimes(2.0 * (varargin_2[2] * pn_a), 2.0 *
887 (varargin_2[2] * pn_b), 2.0 * (varargin_2[2] * n), 2.0 * (varargin_2[2] *
888 pn_c), cd2, -aasq, -qn_c, -qn_d, &q_a[0], &q_a[1], &q_a[2], &q_a[3]);
889 n = sqrt(((cd2 * cd2 + aasq * aasq) + qn_c * qn_c) + qn_d * qn_d);
890 pn_d = cd2 / n;
891 pnCorrected_a = aasq / n;
892 pnCorrected_b = qn_c / n;
893 pnCorrected_c = qn_d / n;
894 ab2 = pn_d * pnCorrected_a * 2.0;
895 ac2 = pn_d * pnCorrected_b * 2.0;
896 ad2 = pn_d * pnCorrected_c * 2.0;
897 bc2 = pnCorrected_a * pnCorrected_b * 2.0;
898 bd2 = pnCorrected_a * pnCorrected_c * 2.0;
899 cd2 = pnCorrected_b * pnCorrected_c * 2.0;
900 aasq = pn_d * pn_d * 2.0 - 1.0;
901 rmat[0] = pnCorrected_a * pnCorrected_a * 2.0 + aasq;
902 rmat[3] = bc2 + ad2;
903 rmat[6] = bd2 - ac2;
904 rmat[1] = bc2 - ad2;
905 rmat[4] = pnCorrected_b * pnCorrected_b * 2.0 + aasq;
906 rmat[7] = cd2 + ab2;
907 rmat[2] = bd2 + ac2;
908 rmat[5] = cd2 - ab2;
909 rmat[8] = pnCorrected_c * pnCorrected_c * 2.0 + aasq;
910 for (b_k = 0; b_k < 3; b_k++) {
911 alpha[b_k] = q_a[(b_k + 2) - 1];
912 subsa_idx_1 = b_k + 1;
913 R[subsa_idx_1 - 1] = rmat[(subsa_idx_1 - 1) * 3];
914 subsa_idx_1 = b_k + 1;
915 R[subsa_idx_1 + 2] = rmat[(subsa_idx_1 - 1) * 3 + 1];
916 subsa_idx_1 = b_k + 1;
917 R[subsa_idx_1 + 5] = rmat[(subsa_idx_1 - 1) * 3 + 2];
918 }
919}
920
921static void cartesian_trajec_emxInit_real_T(emxArray_real_T_cartesian_tra_T
922 **pEmxArray, int32_T numDimensions)
923{
924 emxArray_real_T_cartesian_tra_T *emxArray;
925 *pEmxArray = (emxArray_real_T_cartesian_tra_T *)malloc(sizeof
926 (emxArray_real_T_cartesian_tra_T));
927 emxArray = *pEmxArray;
928 emxArray->data = (real_T *)NULL;
929 emxArray->numDimensions = numDimensions;
930 emxArray->size = (int32_T *)malloc(sizeof(int32_T) * numDimensions);
931 emxArray->allocatedSize = 0;
932 emxArray->canFreeData = true;
933 for (cartesian_trajectory_planner_B.i_j = 0;
934 cartesian_trajectory_planner_B.i_j < numDimensions;
935 cartesian_trajectory_planner_B.i_j++) {
936 emxArray->size[cartesian_trajectory_planner_B.i_j] = 0;
937 }
938}
939
940static void cartes_emxEnsureCapacity_real_T(emxArray_real_T_cartesian_tra_T
941 *emxArray, int32_T oldNumel)
942{
943 void *newData;
944 if (oldNumel < 0) {
945 oldNumel = 0;
946 }
947
948 cartesian_trajectory_planner_B.newNumel_j = 1;
949 for (cartesian_trajectory_planner_B.i_ee = 0;
950 cartesian_trajectory_planner_B.i_ee < emxArray->numDimensions;
951 cartesian_trajectory_planner_B.i_ee++) {
952 cartesian_trajectory_planner_B.newNumel_j *= emxArray->
953 size[cartesian_trajectory_planner_B.i_ee];
954 }
955
956 if (cartesian_trajectory_planner_B.newNumel_j > emxArray->allocatedSize) {
957 cartesian_trajectory_planner_B.i_ee = emxArray->allocatedSize;
958 if (cartesian_trajectory_planner_B.i_ee < 16) {
959 cartesian_trajectory_planner_B.i_ee = 16;
960 }
961
962 while (cartesian_trajectory_planner_B.i_ee <
963 cartesian_trajectory_planner_B.newNumel_j) {
964 if (cartesian_trajectory_planner_B.i_ee > 1073741823) {
965 cartesian_trajectory_planner_B.i_ee = MAX_int32_T;
966 } else {
967 cartesian_trajectory_planner_B.i_ee <<= 1;
968 }
969 }
970
971 newData = calloc(static_cast<uint32_T>(cartesian_trajectory_planner_B.i_ee),
972 sizeof(real_T));
973 if (emxArray->data != NULL) {
974 memcpy(newData, emxArray->data, sizeof(real_T) * oldNumel);
975 if (emxArray->canFreeData) {
976 free(emxArray->data);
977 }
978 }
979
980 emxArray->data = (real_T *)newData;
981 emxArray->allocatedSize = cartesian_trajectory_planner_B.i_ee;
982 emxArray->canFreeData = true;
983 }
984}
985
986static void cartesian_trajec_emxInit_char_T(emxArray_char_T_cartesian_tra_T
987 **pEmxArray, int32_T numDimensions)
988{
989 emxArray_char_T_cartesian_tra_T *emxArray;
990 *pEmxArray = (emxArray_char_T_cartesian_tra_T *)malloc(sizeof
991 (emxArray_char_T_cartesian_tra_T));
992 emxArray = *pEmxArray;
993 emxArray->data = (char_T *)NULL;
994 emxArray->numDimensions = numDimensions;
995 emxArray->size = (int32_T *)malloc(sizeof(int32_T) * numDimensions);
996 emxArray->allocatedSize = 0;
997 emxArray->canFreeData = true;
998 for (cartesian_trajectory_planner_B.i_d = 0;
999 cartesian_trajectory_planner_B.i_d < numDimensions;
1000 cartesian_trajectory_planner_B.i_d++) {
1001 emxArray->size[cartesian_trajectory_planner_B.i_d] = 0;
1002 }
1003}
1004
1005static void cartes_emxEnsureCapacity_char_T(emxArray_char_T_cartesian_tra_T
1006 *emxArray, int32_T oldNumel)
1007{
1008 void *newData;
1009 if (oldNumel < 0) {
1010 oldNumel = 0;
1011 }
1012
1013 cartesian_trajectory_planner_B.newNumel = 1;
1014 for (cartesian_trajectory_planner_B.i_o = 0;
1015 cartesian_trajectory_planner_B.i_o < emxArray->numDimensions;
1016 cartesian_trajectory_planner_B.i_o++) {
1017 cartesian_trajectory_planner_B.newNumel *= emxArray->
1018 size[cartesian_trajectory_planner_B.i_o];
1019 }
1020
1021 if (cartesian_trajectory_planner_B.newNumel > emxArray->allocatedSize) {
1022 cartesian_trajectory_planner_B.i_o = emxArray->allocatedSize;
1023 if (cartesian_trajectory_planner_B.i_o < 16) {
1024 cartesian_trajectory_planner_B.i_o = 16;
1025 }
1026
1027 while (cartesian_trajectory_planner_B.i_o <
1028 cartesian_trajectory_planner_B.newNumel) {
1029 if (cartesian_trajectory_planner_B.i_o > 1073741823) {
1030 cartesian_trajectory_planner_B.i_o = MAX_int32_T;
1031 } else {
1032 cartesian_trajectory_planner_B.i_o <<= 1;
1033 }
1034 }
1035
1036 newData = calloc(static_cast<uint32_T>(cartesian_trajectory_planner_B.i_o),
1037 sizeof(char_T));
1038 if (emxArray->data != NULL) {
1039 memcpy(newData, emxArray->data, sizeof(char_T) * oldNumel);
1040 if (emxArray->canFreeData) {
1041 free(emxArray->data);
1042 }
1043 }
1044
1045 emxArray->data = (char_T *)newData;
1046 emxArray->allocatedSize = cartesian_trajectory_planner_B.i_o;
1047 emxArray->canFreeData = true;
1048 }
1049}
1050
1051static void cartesian_trajec_emxFree_real_T(emxArray_real_T_cartesian_tra_T
1052 **pEmxArray)
1053{
1054 if (*pEmxArray != (emxArray_real_T_cartesian_tra_T *)NULL) {
1055 if (((*pEmxArray)->data != (real_T *)NULL) && (*pEmxArray)->canFreeData) {
1056 free((*pEmxArray)->data);
1057 }
1058
1059 free((*pEmxArray)->size);
1060 free(*pEmxArray);
1061 *pEmxArray = (emxArray_real_T_cartesian_tra_T *)NULL;
1062 }
1063}
1064
1065static void cartesian_trajec_emxFree_char_T(emxArray_char_T_cartesian_tra_T
1066 **pEmxArray)
1067{
1068 if (*pEmxArray != (emxArray_char_T_cartesian_tra_T *)NULL) {
1069 if (((*pEmxArray)->data != (char_T *)NULL) && (*pEmxArray)->canFreeData) {
1070 free((*pEmxArray)->data);
1071 }
1072
1073 free((*pEmxArray)->size);
1074 free(*pEmxArray);
1075 *pEmxArray = (emxArray_char_T_cartesian_tra_T *)NULL;
1076 }
1077}
1078
1079static void car_inverseKinematics_setupImpl(b_inverseKinematics_cartesian_T *obj,
1080 f_robotics_manip_internal_IKE_T *iobj_0)
1081{
1082 real_T n;
1083 emxArray_real_T_cartesian_tra_T *A;
1084 emxArray_real_T_cartesian_tra_T *b;
1085 real_T m;
1086 c_rigidBodyJoint_cartesian__a_T *joint;
1087 real_T pnum;
1088 int32_T d;
1089 int32_T b_i;
1090 emxArray_real_T_cartesian_tra_T *e;
1091 int32_T j;
1092 int32_T p;
1093 emxArray_real_T_cartesian_tra_T *s;
1094 x_robotics_manip_internal_Rig_T *obj_0;
1095 w_robotics_manip_internal_Rig_T *body;
1096 int32_T c;
1097 boolean_T b_bool;
1098 emxArray_char_T_cartesian_tra_T *a;
1099 int8_T b_I[16];
1100 int32_T m_0;
1101 real_T t;
1102 int32_T b_kstr;
1103 char_T b_0[5];
1104 real_T pnum_0;
1105 int32_T loop_ub;
1106 real_T tmp;
1107 real_T w;
1108 static const char_T tmp_0[5] = { 'f', 'i', 'x', 'e', 'd' };
1109
1110 int32_T exitg1;
1111 cartesian_trajec_emxInit_real_T(&A, 2);
1112 n = obj->RigidBodyTreeInternal->PositionNumber;
1113 b_kstr = A->size[0] * A->size[1];
1114 c = static_cast<int32_T>(n);
1115 A->size[0] = c;
1116 loop_ub = static_cast<int32_T>(2.0 * n);
1117 A->size[1] = loop_ub;
1118 cartes_emxEnsureCapacity_real_T(A, b_kstr);
1119 m_0 = loop_ub * c - 1;
1120 for (b_kstr = 0; b_kstr <= m_0; b_kstr++) {
1121 A->data[b_kstr] = 0.0;
1122 }
1123
1124 cartesian_trajec_emxInit_real_T(&b, 1);
1125 b_kstr = b->size[0];
1126 b->size[0] = loop_ub;
1127 cartes_emxEnsureCapacity_real_T(b, b_kstr);
1128 for (b_kstr = 0; b_kstr < loop_ub; b_kstr++) {
1129 b->data[b_kstr] = 0.0;
1130 }
1131
1132 n = 1.0;
1133 m = 1.0;
1134 pnum = obj->RigidBodyTreeInternal->NumBodies;
1135 d = static_cast<int32_T>(pnum) - 1;
1136 cartesian_trajec_emxInit_real_T(&e, 2);
1137 cartesian_trajec_emxInit_real_T(&s, 2);
1138 cartesian_trajec_emxInit_char_T(&a, 2);
1139 if (0 <= d) {
1140 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
1141 b_0[b_kstr] = tmp_0[b_kstr];
1142 }
1143 }
1144
1145 for (b_i = 0; b_i <= d; b_i++) {
1146 body = obj->RigidBodyTreeInternal->Bodies[b_i];
1147 joint = body->JointInternal;
1148 pnum = joint->PositionNumber;
1149 b_kstr = a->size[0] * a->size[1];
1150 a->size[0] = 1;
1151 a->size[1] = joint->Type->size[1];
1152 cartes_emxEnsureCapacity_char_T(a, b_kstr);
1153 loop_ub = joint->Type->size[0] * joint->Type->size[1] - 1;
1154 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
1155 a->data[b_kstr] = joint->Type->data[b_kstr];
1156 }
1157
1158 b_bool = false;
1159 if (a->size[1] == 5) {
1160 b_kstr = 1;
1161 do {
1162 exitg1 = 0;
1163 if (b_kstr - 1 < 5) {
1164 loop_ub = b_kstr - 1;
1165 if (a->data[loop_ub] != b_0[loop_ub]) {
1166 exitg1 = 1;
1167 } else {
1168 b_kstr++;
1169 }
1170 } else {
1171 b_bool = true;
1172 exitg1 = 1;
1173 }
1174 } while (exitg1 == 0);
1175 }
1176
1177 if (!b_bool) {
1178 tmp = (n + pnum) - 1.0;
1179 if (n > tmp) {
1180 j = 0;
1181 } else {
1182 j = static_cast<int32_T>(n) - 1;
1183 }
1184
1185 w = m + pnum;
1186 if (m > w - 1.0) {
1187 p = 0;
1188 } else {
1189 p = static_cast<int32_T>(m) - 1;
1190 }
1191
1192 if (pnum < 0.0) {
1193 t = 0.0;
1194 pnum_0 = 0.0;
1195 } else {
1196 t = pnum;
1197 pnum_0 = pnum;
1198 }
1199
1200 m_0 = static_cast<int32_T>(pnum_0) - 1;
1201 b_kstr = s->size[0] * s->size[1];
1202 c = static_cast<int32_T>(t);
1203 s->size[0] = c;
1204 s->size[1] = c;
1205 cartes_emxEnsureCapacity_real_T(s, b_kstr);
1206 loop_ub = c * c - 1;
1207 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
1208 s->data[b_kstr] = 0.0;
1209 }
1210
1211 if (c > 0) {
1212 for (b_kstr = 0; b_kstr <= m_0; b_kstr++) {
1213 s->data[b_kstr + s->size[0] * b_kstr] = 1.0;
1214 }
1215 }
1216
1217 loop_ub = s->size[1];
1218 for (b_kstr = 0; b_kstr < loop_ub; b_kstr++) {
1219 m_0 = s->size[0];
1220 for (c = 0; c < m_0; c++) {
1221 A->data[(j + c) + A->size[0] * (p + b_kstr)] = s->data[s->size[0] *
1222 b_kstr + c];
1223 }
1224 }
1225
1226 if (n > tmp) {
1227 j = 0;
1228 } else {
1229 j = static_cast<int32_T>(n) - 1;
1230 }
1231
1232 tmp = 2.0 * pnum + m;
1233 if (w > tmp - 1.0) {
1234 p = 0;
1235 } else {
1236 p = static_cast<int32_T>(w) - 1;
1237 }
1238
1239 if (pnum < 0.0) {
1240 t = 0.0;
1241 pnum_0 = 0.0;
1242 } else {
1243 t = pnum;
1244 pnum_0 = pnum;
1245 }
1246
1247 m_0 = static_cast<int32_T>(pnum_0) - 1;
1248 b_kstr = s->size[0] * s->size[1];
1249 c = static_cast<int32_T>(t);
1250 s->size[0] = c;
1251 s->size[1] = c;
1252 cartes_emxEnsureCapacity_real_T(s, b_kstr);
1253 loop_ub = c * c - 1;
1254 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
1255 s->data[b_kstr] = 0.0;
1256 }
1257
1258 if (c > 0) {
1259 for (b_kstr = 0; b_kstr <= m_0; b_kstr++) {
1260 s->data[b_kstr + s->size[0] * b_kstr] = 1.0;
1261 }
1262 }
1263
1264 loop_ub = s->size[1];
1265 for (b_kstr = 0; b_kstr < loop_ub; b_kstr++) {
1266 m_0 = s->size[0];
1267 for (c = 0; c < m_0; c++) {
1268 A->data[(j + c) + A->size[0] * (p + b_kstr)] = -s->data[s->size[0] *
1269 b_kstr + c];
1270 }
1271 }
1272
1273 b_kstr = e->size[0] * e->size[1];
1274 e->size[0] = joint->PositionLimitsInternal->size[0];
1275 e->size[1] = 2;
1276 cartes_emxEnsureCapacity_real_T(e, b_kstr);
1277 loop_ub = joint->PositionLimitsInternal->size[0] *
1278 joint->PositionLimitsInternal->size[1] - 1;
1279 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
1280 e->data[b_kstr] = joint->PositionLimitsInternal->data[b_kstr];
1281 }
1282
1283 b->data[static_cast<int32_T>(m) - 1] = e->data[1];
1284 b_kstr = e->size[0] * e->size[1];
1285 e->size[0] = joint->PositionLimitsInternal->size[0];
1286 e->size[1] = 2;
1287 cartes_emxEnsureCapacity_real_T(e, b_kstr);
1288 loop_ub = joint->PositionLimitsInternal->size[0] *
1289 joint->PositionLimitsInternal->size[1] - 1;
1290 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
1291 e->data[b_kstr] = joint->PositionLimitsInternal->data[b_kstr];
1292 }
1293
1294 b->data[static_cast<int32_T>(m + 1.0) - 1] = -e->data[0];
1295 m = tmp;
1296 }
1297
1298 n += pnum;
1299 }
1300
1301 cartesian_trajec_emxFree_real_T(&s);
1302 b_kstr = A->size[0] * A->size[1];
1303 c = obj->Solver->ConstraintMatrix->size[0] * obj->Solver->
1304 ConstraintMatrix->size[1];
1305 obj->Solver->ConstraintMatrix->size[0] = A->size[0];
1306 obj->Solver->ConstraintMatrix->size[1] = A->size[1];
1307 cartes_emxEnsureCapacity_real_T(obj->Solver->ConstraintMatrix, c);
1308 loop_ub = b_kstr - 1;
1309 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
1310 obj->Solver->ConstraintMatrix->data[b_kstr] = A->data[b_kstr];
1311 }
1312
1313 cartesian_trajec_emxFree_real_T(&A);
1314 b_kstr = obj->Solver->ConstraintBound->size[0];
1315 obj->Solver->ConstraintBound->size[0] = b->size[0];
1316 cartes_emxEnsureCapacity_real_T(obj->Solver->ConstraintBound, b_kstr);
1317 loop_ub = b->size[0];
1318 for (b_kstr = 0; b_kstr < loop_ub; b_kstr++) {
1319 obj->Solver->ConstraintBound->data[b_kstr] = b->data[b_kstr];
1320 }
1321
1322 obj_0 = obj->RigidBodyTreeInternal;
1323 b_kstr = e->size[0] * e->size[1];
1324 e->size[0] = static_cast<int32_T>(obj_0->PositionNumber);
1325 e->size[1] = 2;
1326 cartes_emxEnsureCapacity_real_T(e, b_kstr);
1327 loop_ub = (static_cast<int32_T>(obj_0->PositionNumber) << 1) - 1;
1328 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
1329 e->data[b_kstr] = 0.0;
1330 }
1331
1332 n = 1.0;
1333 m = obj_0->NumBodies;
1334 c = static_cast<int32_T>(m) - 1;
1335 if (0 <= c) {
1336 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
1337 b_0[b_kstr] = tmp_0[b_kstr];
1338 }
1339 }
1340
1341 for (b_i = 0; b_i <= c; b_i++) {
1342 body = obj_0->Bodies[b_i];
1343 b_kstr = a->size[0] * a->size[1];
1344 a->size[0] = 1;
1345 a->size[1] = body->JointInternal->Type->size[1];
1346 cartes_emxEnsureCapacity_char_T(a, b_kstr);
1347 loop_ub = body->JointInternal->Type->size[0] * body->JointInternal->
1348 Type->size[1] - 1;
1349 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
1350 a->data[b_kstr] = body->JointInternal->Type->data[b_kstr];
1351 }
1352
1353 b_bool = false;
1354 if (a->size[1] == 5) {
1355 b_kstr = 1;
1356 do {
1357 exitg1 = 0;
1358 if (b_kstr - 1 < 5) {
1359 loop_ub = b_kstr - 1;
1360 if (a->data[loop_ub] != b_0[loop_ub]) {
1361 exitg1 = 1;
1362 } else {
1363 b_kstr++;
1364 }
1365 } else {
1366 b_bool = true;
1367 exitg1 = 1;
1368 }
1369 } while (exitg1 == 0);
1370 }
1371
1372 if (!b_bool) {
1373 pnum = body->JointInternal->PositionNumber;
1374 tmp = n + pnum;
1375 if (n > tmp - 1.0) {
1376 d = 0;
1377 } else {
1378 d = static_cast<int32_T>(n) - 1;
1379 }
1380
1381 joint = body->JointInternal;
1382 loop_ub = joint->PositionLimitsInternal->size[0];
1383 for (b_kstr = 0; b_kstr < loop_ub; b_kstr++) {
1384 e->data[d + b_kstr] = joint->PositionLimitsInternal->data[b_kstr];
1385 }
1386
1387 loop_ub = joint->PositionLimitsInternal->size[0];
1388 for (b_kstr = 0; b_kstr < loop_ub; b_kstr++) {
1389 e->data[(d + b_kstr) + e->size[0]] = joint->PositionLimitsInternal->
1390 data[b_kstr + joint->PositionLimitsInternal->size[0]];
1391 }
1392
1393 n = tmp;
1394 }
1395 }
1396
1397 cartesian_trajec_emxFree_char_T(&a);
1398 b_kstr = obj->Limits->size[0] * obj->Limits->size[1];
1399 obj->Limits->size[0] = e->size[0];
1400 obj->Limits->size[1] = 2;
1401 cartes_emxEnsureCapacity_real_T(obj->Limits, b_kstr);
1402 loop_ub = e->size[0] * e->size[1] - 1;
1403 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
1404 obj->Limits->data[b_kstr] = e->data[b_kstr];
1405 }
1406
1407 cartesian_trajec_emxFree_real_T(&e);
1408 obj->Solver->ExtraArgs = iobj_0;
1409 for (b_kstr = 0; b_kstr < 36; b_kstr++) {
1410 obj->Solver->ExtraArgs->WeightMatrix[b_kstr] = 0.0;
1411 }
1412
1413 obj->Solver->ExtraArgs->Robot = obj->RigidBodyTreeInternal;
1414 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
1415 b_I[b_kstr] = 0;
1416 }
1417
1418 b_I[0] = 1;
1419 b_I[5] = 1;
1420 b_I[10] = 1;
1421 b_I[15] = 1;
1422 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
1423 obj->Solver->ExtraArgs->Tform[b_kstr] = b_I[b_kstr];
1424 }
1425
1426 obj->Solver->ExtraArgs->BodyName->size[0] = 1;
1427 obj->Solver->ExtraArgs->BodyName->size[1] = 0;
1428 b_kstr = obj->Solver->ExtraArgs->ErrTemp->size[0];
1429 obj->Solver->ExtraArgs->ErrTemp->size[0] = 6;
1430 cartes_emxEnsureCapacity_real_T(obj->Solver->ExtraArgs->ErrTemp, b_kstr);
1431 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
1432 obj->Solver->ExtraArgs->ErrTemp->data[b_kstr] = 0.0;
1433 }
1434
1435 obj->Solver->ExtraArgs->CostTemp = 0.0;
1436 b_kstr = b->size[0];
1437 b->size[0] = static_cast<int32_T>(obj->RigidBodyTreeInternal->PositionNumber);
1438 cartes_emxEnsureCapacity_real_T(b, b_kstr);
1439 loop_ub = static_cast<int32_T>(obj->RigidBodyTreeInternal->PositionNumber);
1440 for (b_kstr = 0; b_kstr < loop_ub; b_kstr++) {
1441 b->data[b_kstr] = 0.0;
1442 }
1443
1444 b_kstr = obj->Solver->ExtraArgs->GradTemp->size[0];
1445 obj->Solver->ExtraArgs->GradTemp->size[0] = b->size[0];
1446 cartes_emxEnsureCapacity_real_T(obj->Solver->ExtraArgs->GradTemp, b_kstr);
1447 loop_ub = b->size[0];
1448 for (b_kstr = 0; b_kstr < loop_ub; b_kstr++) {
1449 obj->Solver->ExtraArgs->GradTemp->data[b_kstr] = b->data[b_kstr];
1450 }
1451
1452 cartesian_trajec_emxFree_real_T(&b);
1453}
1454
1455static void c_inverseKinematics_setPoseGoal(b_inverseKinematics_cartesian_T *obj,
1456 const real_T tform[16], const real_T weights[6])
1457{
1458 real_T weightMatrix[36];
1459 f_robotics_manip_internal_IKE_T *args;
1460 int32_T b_j;
1461 static const char_T tmp[11] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
1462 'e', 'e' };
1463
1464 memset(&weightMatrix[0], 0, 36U * sizeof(real_T));
1465 for (b_j = 0; b_j < 6; b_j++) {
1466 weightMatrix[b_j + 6 * b_j] = weights[b_j];
1467 }
1468
1469 args = obj->Solver->ExtraArgs;
1470 for (b_j = 0; b_j < 36; b_j++) {
1471 args->WeightMatrix[b_j] = weightMatrix[b_j];
1472 }
1473
1474 b_j = args->BodyName->size[0] * args->BodyName->size[1];
1475 args->BodyName->size[0] = 1;
1476 args->BodyName->size[1] = 11;
1477 cartes_emxEnsureCapacity_char_T(args->BodyName, b_j);
1478 for (b_j = 0; b_j < 11; b_j++) {
1479 args->BodyName->data[b_j] = tmp[b_j];
1480 }
1481
1482 for (b_j = 0; b_j < 16; b_j++) {
1483 args->Tform[b_j] = tform[b_j];
1484 }
1485}
1486
1487static void RigidBodyTree_validateConfigu_a(x_robotics_manip_internal_Rig_T *obj,
1488 real_T Q[6])
1489{
1490 emxArray_real_T_cartesian_tra_T *limits;
1491 boolean_T ubOK[6];
1492 boolean_T lbOK[6];
1493 real_T k;
1494 w_robotics_manip_internal_Rig_T *body;
1495 real_T pnum;
1496 int32_T c;
1497 int32_T f;
1498 int32_T ii_data[6];
1499 boolean_T b_bool;
1500 emxArray_char_T_cartesian_tra_T *a;
1501 c_rigidBodyJoint_cartesian__a_T *obj_0;
1502 int32_T idx;
1503 int32_T b_kstr;
1504 char_T b[5];
1505 int32_T loop_ub;
1506 emxArray_real_T_cartesian_tra_T *limits_0;
1507 emxArray_real_T_cartesian_tra_T *limits_1;
1508 static const char_T tmp[5] = { 'f', 'i', 'x', 'e', 'd' };
1509
1510 boolean_T guard1 = false;
1511 int32_T exitg1;
1512 boolean_T exitg2;
1513 cartesian_trajec_emxInit_real_T(&limits, 2);
1514 b_kstr = limits->size[0] * limits->size[1];
1515 limits->size[0] = static_cast<int32_T>(obj->PositionNumber);
1516 limits->size[1] = 2;
1517 cartes_emxEnsureCapacity_real_T(limits, b_kstr);
1518 loop_ub = (static_cast<int32_T>(obj->PositionNumber) << 1) - 1;
1519 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
1520 limits->data[b_kstr] = 0.0;
1521 }
1522
1523 k = 1.0;
1524 pnum = obj->NumBodies;
1525 c = static_cast<int32_T>(pnum) - 1;
1526 cartesian_trajec_emxInit_char_T(&a, 2);
1527 if (0 <= c) {
1528 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
1529 b[b_kstr] = tmp[b_kstr];
1530 }
1531 }
1532
1533 for (idx = 0; idx <= c; idx++) {
1534 body = obj->Bodies[idx];
1535 b_kstr = a->size[0] * a->size[1];
1536 a->size[0] = 1;
1537 a->size[1] = body->JointInternal->Type->size[1];
1538 cartes_emxEnsureCapacity_char_T(a, b_kstr);
1539 loop_ub = body->JointInternal->Type->size[0] * body->JointInternal->
1540 Type->size[1] - 1;
1541 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
1542 a->data[b_kstr] = body->JointInternal->Type->data[b_kstr];
1543 }
1544
1545 b_bool = false;
1546 if (a->size[1] == 5) {
1547 b_kstr = 1;
1548 do {
1549 exitg1 = 0;
1550 if (b_kstr - 1 < 5) {
1551 loop_ub = b_kstr - 1;
1552 if (a->data[loop_ub] != b[loop_ub]) {
1553 exitg1 = 1;
1554 } else {
1555 b_kstr++;
1556 }
1557 } else {
1558 b_bool = true;
1559 exitg1 = 1;
1560 }
1561 } while (exitg1 == 0);
1562 }
1563
1564 if (!b_bool) {
1565 pnum = body->JointInternal->PositionNumber;
1566 pnum += k;
1567 if (k > pnum - 1.0) {
1568 f = 0;
1569 } else {
1570 f = static_cast<int32_T>(k) - 1;
1571 }
1572
1573 obj_0 = body->JointInternal;
1574 loop_ub = obj_0->PositionLimitsInternal->size[0];
1575 for (b_kstr = 0; b_kstr < loop_ub; b_kstr++) {
1576 limits->data[f + b_kstr] = obj_0->PositionLimitsInternal->data[b_kstr];
1577 }
1578
1579 loop_ub = obj_0->PositionLimitsInternal->size[0];
1580 for (b_kstr = 0; b_kstr < loop_ub; b_kstr++) {
1581 limits->data[(f + b_kstr) + limits->size[0]] =
1582 obj_0->PositionLimitsInternal->data[b_kstr +
1583 obj_0->PositionLimitsInternal->size[0]];
1584 }
1585
1586 k = pnum;
1587 }
1588 }
1589
1590 cartesian_trajec_emxFree_char_T(&a);
1591 cartesian_trajec_emxInit_real_T(&limits_0, 1);
1592 loop_ub = limits->size[0];
1593 b_kstr = limits_0->size[0];
1594 limits_0->size[0] = loop_ub;
1595 cartes_emxEnsureCapacity_real_T(limits_0, b_kstr);
1596 for (b_kstr = 0; b_kstr < loop_ub; b_kstr++) {
1597 limits_0->data[b_kstr] = limits->data[b_kstr + limits->size[0]] +
1598 4.4408920985006262E-16;
1599 }
1600
1601 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
1602 ubOK[b_kstr] = (Q[b_kstr] <= limits_0->data[b_kstr]);
1603 }
1604
1605 cartesian_trajec_emxFree_real_T(&limits_0);
1606 cartesian_trajec_emxInit_real_T(&limits_1, 1);
1607 loop_ub = limits->size[0];
1608 b_kstr = limits_1->size[0];
1609 limits_1->size[0] = loop_ub;
1610 cartes_emxEnsureCapacity_real_T(limits_1, b_kstr);
1611 for (b_kstr = 0; b_kstr < loop_ub; b_kstr++) {
1612 limits_1->data[b_kstr] = limits->data[b_kstr] - 4.4408920985006262E-16;
1613 }
1614
1615 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
1616 lbOK[b_kstr] = (Q[b_kstr] >= limits_1->data[b_kstr]);
1617 }
1618
1619 cartesian_trajec_emxFree_real_T(&limits_1);
1620 b_bool = true;
1621 idx = 0;
1622 exitg2 = false;
1623 while ((!exitg2) && (idx < 6)) {
1624 if (!ubOK[idx]) {
1625 b_bool = false;
1626 exitg2 = true;
1627 } else {
1628 idx++;
1629 }
1630 }
1631
1632 guard1 = false;
1633 if (b_bool) {
1634 b_bool = true;
1635 idx = 0;
1636 exitg2 = false;
1637 while ((!exitg2) && (idx < 6)) {
1638 if (!lbOK[idx]) {
1639 b_bool = false;
1640 exitg2 = true;
1641 } else {
1642 idx++;
1643 }
1644 }
1645
1646 if (b_bool) {
1647 } else {
1648 guard1 = true;
1649 }
1650 } else {
1651 guard1 = true;
1652 }
1653
1654 if (guard1) {
1655 idx = 0;
1656 b_kstr = 1;
1657 exitg2 = false;
1658 while ((!exitg2) && (b_kstr - 1 < 6)) {
1659 if (!ubOK[b_kstr - 1]) {
1660 idx++;
1661 ii_data[idx - 1] = b_kstr;
1662 if (idx >= 6) {
1663 exitg2 = true;
1664 } else {
1665 b_kstr++;
1666 }
1667 } else {
1668 b_kstr++;
1669 }
1670 }
1671
1672 if (1 > idx) {
1673 idx = 0;
1674 }
1675
1676 for (b_kstr = 0; b_kstr < idx; b_kstr++) {
1677 Q[ii_data[b_kstr] - 1] = limits->data[(ii_data[b_kstr] + limits->size[0])
1678 - 1];
1679 }
1680
1681 idx = 0;
1682 b_kstr = 1;
1683 exitg2 = false;
1684 while ((!exitg2) && (b_kstr - 1 < 6)) {
1685 if (!lbOK[b_kstr - 1]) {
1686 idx++;
1687 ii_data[idx - 1] = b_kstr;
1688 if (idx >= 6) {
1689 exitg2 = true;
1690 } else {
1691 b_kstr++;
1692 }
1693 } else {
1694 b_kstr++;
1695 }
1696 }
1697
1698 if (1 > idx) {
1699 idx = 0;
1700 }
1701
1702 for (b_kstr = 0; b_kstr < idx; b_kstr++) {
1703 Q[ii_data[b_kstr] - 1] = limits->data[ii_data[b_kstr] - 1];
1704 }
1705 }
1706
1707 cartesian_trajec_emxFree_real_T(&limits);
1708}
1709
1710static boolean_T cartesian_trajectory_pla_strcmp(const
1711 emxArray_char_T_cartesian_tra_T *a, const emxArray_char_T_cartesian_tra_T *b)
1712{
1713 boolean_T b_bool;
1714 int32_T kstr;
1715 int32_T b_kstr;
1716 boolean_T d;
1717 int32_T exitg1;
1718 b_bool = false;
1719 d = (a->size[1] == 0);
1720 if (d && (b->size[1] == 0)) {
1721 b_bool = true;
1722 } else if (a->size[1] != b->size[1]) {
1723 } else {
1724 b_kstr = 1;
1725 do {
1726 exitg1 = 0;
1727 if (b_kstr - 1 <= b->size[1] - 1) {
1728 kstr = b_kstr - 1;
1729 if (a->data[kstr] != b->data[kstr]) {
1730 exitg1 = 1;
1731 } else {
1732 b_kstr++;
1733 }
1734 } else {
1735 b_bool = true;
1736 exitg1 = 1;
1737 }
1738 } while (exitg1 == 0);
1739 }
1740
1741 return b_bool;
1742}
1743
1744static void rigidBodyJoint_get_JointAxis_a(const c_rigidBodyJoint_cartesian__a_T
1745 *obj, real_T ax[3])
1746{
1747 emxArray_char_T_cartesian_tra_T *a;
1748 static const char_T tmp[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
1749
1750 static const char_T tmp_0[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
1751
1752 boolean_T guard1 = false;
1753 int32_T exitg1;
1754 cartesian_trajec_emxInit_char_T(&a, 2);
1755 cartesian_trajectory_planner_B.b_kstr_hz = a->size[0] * a->size[1];
1756 a->size[0] = 1;
1757 a->size[1] = obj->Type->size[1];
1758 cartes_emxEnsureCapacity_char_T(a, cartesian_trajectory_planner_B.b_kstr_hz);
1759 cartesian_trajectory_planner_B.loop_ub_b = obj->Type->size[0] * obj->
1760 Type->size[1] - 1;
1761 for (cartesian_trajectory_planner_B.b_kstr_hz = 0;
1762 cartesian_trajectory_planner_B.b_kstr_hz <=
1763 cartesian_trajectory_planner_B.loop_ub_b;
1764 cartesian_trajectory_planner_B.b_kstr_hz++) {
1765 a->data[cartesian_trajectory_planner_B.b_kstr_hz] = obj->Type->
1766 data[cartesian_trajectory_planner_B.b_kstr_hz];
1767 }
1768
1769 for (cartesian_trajectory_planner_B.b_kstr_hz = 0;
1770 cartesian_trajectory_planner_B.b_kstr_hz < 8;
1771 cartesian_trajectory_planner_B.b_kstr_hz++) {
1772 cartesian_trajectory_planner_B.b_fx[cartesian_trajectory_planner_B.b_kstr_hz]
1773 = tmp[cartesian_trajectory_planner_B.b_kstr_hz];
1774 }
1775
1776 cartesian_trajectory_planner_B.b_bool_m = false;
1777 if (a->size[1] == 8) {
1778 cartesian_trajectory_planner_B.b_kstr_hz = 1;
1779 do {
1780 exitg1 = 0;
1781 if (cartesian_trajectory_planner_B.b_kstr_hz - 1 < 8) {
1782 cartesian_trajectory_planner_B.loop_ub_b =
1783 cartesian_trajectory_planner_B.b_kstr_hz - 1;
1784 if (a->data[cartesian_trajectory_planner_B.loop_ub_b] !=
1785 cartesian_trajectory_planner_B.b_fx[cartesian_trajectory_planner_B.loop_ub_b])
1786 {
1787 exitg1 = 1;
1788 } else {
1789 cartesian_trajectory_planner_B.b_kstr_hz++;
1790 }
1791 } else {
1792 cartesian_trajectory_planner_B.b_bool_m = true;
1793 exitg1 = 1;
1794 }
1795 } while (exitg1 == 0);
1796 }
1797
1798 guard1 = false;
1799 if (cartesian_trajectory_planner_B.b_bool_m) {
1800 guard1 = true;
1801 } else {
1802 cartesian_trajectory_planner_B.b_kstr_hz = a->size[0] * a->size[1];
1803 a->size[0] = 1;
1804 a->size[1] = obj->Type->size[1];
1805 cartes_emxEnsureCapacity_char_T(a, cartesian_trajectory_planner_B.b_kstr_hz);
1806 cartesian_trajectory_planner_B.loop_ub_b = obj->Type->size[0] * obj->
1807 Type->size[1] - 1;
1808 for (cartesian_trajectory_planner_B.b_kstr_hz = 0;
1809 cartesian_trajectory_planner_B.b_kstr_hz <=
1810 cartesian_trajectory_planner_B.loop_ub_b;
1811 cartesian_trajectory_planner_B.b_kstr_hz++) {
1812 a->data[cartesian_trajectory_planner_B.b_kstr_hz] = obj->Type->
1813 data[cartesian_trajectory_planner_B.b_kstr_hz];
1814 }
1815
1816 for (cartesian_trajectory_planner_B.b_kstr_hz = 0;
1817 cartesian_trajectory_planner_B.b_kstr_hz < 9;
1818 cartesian_trajectory_planner_B.b_kstr_hz++) {
1819 cartesian_trajectory_planner_B.b_f[cartesian_trajectory_planner_B.b_kstr_hz]
1820 = tmp_0[cartesian_trajectory_planner_B.b_kstr_hz];
1821 }
1822
1823 cartesian_trajectory_planner_B.b_bool_m = false;
1824 if (a->size[1] == 9) {
1825 cartesian_trajectory_planner_B.b_kstr_hz = 1;
1826 do {
1827 exitg1 = 0;
1828 if (cartesian_trajectory_planner_B.b_kstr_hz - 1 < 9) {
1829 cartesian_trajectory_planner_B.loop_ub_b =
1830 cartesian_trajectory_planner_B.b_kstr_hz - 1;
1831 if (a->data[cartesian_trajectory_planner_B.loop_ub_b] !=
1832 cartesian_trajectory_planner_B.b_f[cartesian_trajectory_planner_B.loop_ub_b])
1833 {
1834 exitg1 = 1;
1835 } else {
1836 cartesian_trajectory_planner_B.b_kstr_hz++;
1837 }
1838 } else {
1839 cartesian_trajectory_planner_B.b_bool_m = true;
1840 exitg1 = 1;
1841 }
1842 } while (exitg1 == 0);
1843 }
1844
1845 if (cartesian_trajectory_planner_B.b_bool_m) {
1846 guard1 = true;
1847 } else {
1848 ax[0] = (rtNaN);
1849 ax[1] = (rtNaN);
1850 ax[2] = (rtNaN);
1851 }
1852 }
1853
1854 if (guard1) {
1855 ax[0] = obj->JointAxisInternal[0];
1856 ax[1] = obj->JointAxisInternal[1];
1857 ax[2] = obj->JointAxisInternal[2];
1858 }
1859
1860 cartesian_trajec_emxFree_char_T(&a);
1861}
1862
1863static void cartesian_trajectory_planne_cat(real_T varargin_1, real_T varargin_2,
1864 real_T varargin_3, real_T varargin_4, real_T varargin_5, real_T varargin_6,
1865 real_T varargin_7, real_T varargin_8, real_T varargin_9, real_T y[9])
1866{
1867 y[0] = varargin_1;
1868 y[1] = varargin_2;
1869 y[2] = varargin_3;
1870 y[3] = varargin_4;
1871 y[4] = varargin_5;
1872 y[5] = varargin_6;
1873 y[6] = varargin_7;
1874 y[7] = varargin_8;
1875 y[8] = varargin_9;
1876}
1877
1878static void rigidBodyJoint_transformBodyT_a(const
1879 c_rigidBodyJoint_cartesian__a_T *obj, const real_T q_data[], const int32_T
1880 *q_size, real_T T[16])
1881{
1882 emxArray_char_T_cartesian_tra_T *switch_expression;
1883 static const char_T tmp[5] = { 'f', 'i', 'x', 'e', 'd' };
1884
1885 static const char_T tmp_0[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
1886
1887 int32_T exitg1;
1888 cartesian_trajec_emxInit_char_T(&switch_expression, 2);
1889 cartesian_trajectory_planner_B.b_kstr_h = switch_expression->size[0] *
1890 switch_expression->size[1];
1891 switch_expression->size[0] = 1;
1892 switch_expression->size[1] = obj->Type->size[1];
1893 cartes_emxEnsureCapacity_char_T(switch_expression,
1894 cartesian_trajectory_planner_B.b_kstr_h);
1895 cartesian_trajectory_planner_B.loop_ub_k = obj->Type->size[0] * obj->
1896 Type->size[1] - 1;
1897 for (cartesian_trajectory_planner_B.b_kstr_h = 0;
1898 cartesian_trajectory_planner_B.b_kstr_h <=
1899 cartesian_trajectory_planner_B.loop_ub_k;
1900 cartesian_trajectory_planner_B.b_kstr_h++) {
1901 switch_expression->data[cartesian_trajectory_planner_B.b_kstr_h] = obj->
1902 Type->data[cartesian_trajectory_planner_B.b_kstr_h];
1903 }
1904
1905 for (cartesian_trajectory_planner_B.b_kstr_h = 0;
1906 cartesian_trajectory_planner_B.b_kstr_h < 5;
1907 cartesian_trajectory_planner_B.b_kstr_h++) {
1908 cartesian_trajectory_planner_B.b_jo[cartesian_trajectory_planner_B.b_kstr_h]
1909 = tmp[cartesian_trajectory_planner_B.b_kstr_h];
1910 }
1911
1912 cartesian_trajectory_planner_B.b_bool_i = false;
1913 if (switch_expression->size[1] == 5) {
1914 cartesian_trajectory_planner_B.b_kstr_h = 1;
1915 do {
1916 exitg1 = 0;
1917 if (cartesian_trajectory_planner_B.b_kstr_h - 1 < 5) {
1918 cartesian_trajectory_planner_B.loop_ub_k =
1919 cartesian_trajectory_planner_B.b_kstr_h - 1;
1920 if (switch_expression->data[cartesian_trajectory_planner_B.loop_ub_k] !=
1921 cartesian_trajectory_planner_B.b_jo[cartesian_trajectory_planner_B.loop_ub_k])
1922 {
1923 exitg1 = 1;
1924 } else {
1925 cartesian_trajectory_planner_B.b_kstr_h++;
1926 }
1927 } else {
1928 cartesian_trajectory_planner_B.b_bool_i = true;
1929 exitg1 = 1;
1930 }
1931 } while (exitg1 == 0);
1932 }
1933
1934 if (cartesian_trajectory_planner_B.b_bool_i) {
1935 cartesian_trajectory_planner_B.b_kstr_h = 0;
1936 } else {
1937 for (cartesian_trajectory_planner_B.b_kstr_h = 0;
1938 cartesian_trajectory_planner_B.b_kstr_h < 8;
1939 cartesian_trajectory_planner_B.b_kstr_h++) {
1940 cartesian_trajectory_planner_B.b_c[cartesian_trajectory_planner_B.b_kstr_h]
1941 = tmp_0[cartesian_trajectory_planner_B.b_kstr_h];
1942 }
1943
1944 cartesian_trajectory_planner_B.b_bool_i = false;
1945 if (switch_expression->size[1] == 8) {
1946 cartesian_trajectory_planner_B.b_kstr_h = 1;
1947 do {
1948 exitg1 = 0;
1949 if (cartesian_trajectory_planner_B.b_kstr_h - 1 < 8) {
1950 cartesian_trajectory_planner_B.loop_ub_k =
1951 cartesian_trajectory_planner_B.b_kstr_h - 1;
1952 if (switch_expression->data[cartesian_trajectory_planner_B.loop_ub_k]
1953 !=
1954 cartesian_trajectory_planner_B.b_c[cartesian_trajectory_planner_B.loop_ub_k])
1955 {
1956 exitg1 = 1;
1957 } else {
1958 cartesian_trajectory_planner_B.b_kstr_h++;
1959 }
1960 } else {
1961 cartesian_trajectory_planner_B.b_bool_i = true;
1962 exitg1 = 1;
1963 }
1964 } while (exitg1 == 0);
1965 }
1966
1967 if (cartesian_trajectory_planner_B.b_bool_i) {
1968 cartesian_trajectory_planner_B.b_kstr_h = 1;
1969 } else {
1970 cartesian_trajectory_planner_B.b_kstr_h = -1;
1971 }
1972 }
1973
1974 cartesian_trajec_emxFree_char_T(&switch_expression);
1975 switch (cartesian_trajectory_planner_B.b_kstr_h) {
1976 case 0:
1977 memset(&cartesian_trajectory_planner_B.TJ[0], 0, sizeof(real_T) << 4U);
1978 cartesian_trajectory_planner_B.TJ[0] = 1.0;
1979 cartesian_trajectory_planner_B.TJ[5] = 1.0;
1980 cartesian_trajectory_planner_B.TJ[10] = 1.0;
1981 cartesian_trajectory_planner_B.TJ[15] = 1.0;
1982 break;
1983
1984 case 1:
1985 rigidBodyJoint_get_JointAxis_a(obj, cartesian_trajectory_planner_B.v_b);
1986 cartesian_trajectory_planner_B.loop_ub_k = (*q_size != 0) - 1;
1987 cartesian_trajectory_planner_B.result_data[0] =
1988 cartesian_trajectory_planner_B.v_b[0];
1989 cartesian_trajectory_planner_B.result_data[1] =
1990 cartesian_trajectory_planner_B.v_b[1];
1991 cartesian_trajectory_planner_B.result_data[2] =
1992 cartesian_trajectory_planner_B.v_b[2];
1993 for (cartesian_trajectory_planner_B.b_kstr_h = 0;
1994 cartesian_trajectory_planner_B.b_kstr_h <=
1995 cartesian_trajectory_planner_B.loop_ub_k;
1996 cartesian_trajectory_planner_B.b_kstr_h++) {
1997 cartesian_trajectory_planner_B.result_data[3] = q_data[0];
1998 }
1999
2000 cartesian_trajectory_planner_B.cth = 1.0 / sqrt
2001 ((cartesian_trajectory_planner_B.result_data[0] *
2002 cartesian_trajectory_planner_B.result_data[0] +
2003 cartesian_trajectory_planner_B.result_data[1] *
2004 cartesian_trajectory_planner_B.result_data[1]) +
2005 cartesian_trajectory_planner_B.result_data[2] *
2006 cartesian_trajectory_planner_B.result_data[2]);
2007 cartesian_trajectory_planner_B.v_b[0] =
2008 cartesian_trajectory_planner_B.result_data[0] *
2009 cartesian_trajectory_planner_B.cth;
2010 cartesian_trajectory_planner_B.v_b[1] =
2011 cartesian_trajectory_planner_B.result_data[1] *
2012 cartesian_trajectory_planner_B.cth;
2013 cartesian_trajectory_planner_B.v_b[2] =
2014 cartesian_trajectory_planner_B.result_data[2] *
2015 cartesian_trajectory_planner_B.cth;
2016 cartesian_trajectory_planner_B.cth = cos
2017 (cartesian_trajectory_planner_B.result_data[3]);
2018 cartesian_trajectory_planner_B.sth = sin
2019 (cartesian_trajectory_planner_B.result_data[3]);
2020 cartesian_trajectory_planner_B.tempR_tmp_o =
2021 cartesian_trajectory_planner_B.v_b[1] *
2022 cartesian_trajectory_planner_B.v_b[0] * (1.0 -
2023 cartesian_trajectory_planner_B.cth);
2024 cartesian_trajectory_planner_B.tempR_tmp_c =
2025 cartesian_trajectory_planner_B.v_b[2] * cartesian_trajectory_planner_B.sth;
2026 cartesian_trajectory_planner_B.tempR_tmp_b =
2027 cartesian_trajectory_planner_B.v_b[2] *
2028 cartesian_trajectory_planner_B.v_b[0] * (1.0 -
2029 cartesian_trajectory_planner_B.cth);
2030 cartesian_trajectory_planner_B.tempR_tmp_e =
2031 cartesian_trajectory_planner_B.v_b[1] * cartesian_trajectory_planner_B.sth;
2032 cartesian_trajectory_planner_B.tempR_tmp_dd =
2033 cartesian_trajectory_planner_B.v_b[2] *
2034 cartesian_trajectory_planner_B.v_b[1] * (1.0 -
2035 cartesian_trajectory_planner_B.cth);
2036 cartesian_trajectory_planner_B.sth *= cartesian_trajectory_planner_B.v_b[0];
2037 cartesian_trajectory_planne_cat(cartesian_trajectory_planner_B.v_b[0] *
2038 cartesian_trajectory_planner_B.v_b[0] * (1.0 -
2039 cartesian_trajectory_planner_B.cth) + cartesian_trajectory_planner_B.cth,
2040 cartesian_trajectory_planner_B.tempR_tmp_o -
2041 cartesian_trajectory_planner_B.tempR_tmp_c,
2042 cartesian_trajectory_planner_B.tempR_tmp_b +
2043 cartesian_trajectory_planner_B.tempR_tmp_e,
2044 cartesian_trajectory_planner_B.tempR_tmp_o +
2045 cartesian_trajectory_planner_B.tempR_tmp_c,
2046 cartesian_trajectory_planner_B.v_b[1] *
2047 cartesian_trajectory_planner_B.v_b[1] * (1.0 -
2048 cartesian_trajectory_planner_B.cth) + cartesian_trajectory_planner_B.cth,
2049 cartesian_trajectory_planner_B.tempR_tmp_dd -
2050 cartesian_trajectory_planner_B.sth,
2051 cartesian_trajectory_planner_B.tempR_tmp_b -
2052 cartesian_trajectory_planner_B.tempR_tmp_e,
2053 cartesian_trajectory_planner_B.tempR_tmp_dd +
2054 cartesian_trajectory_planner_B.sth, cartesian_trajectory_planner_B.v_b[2] *
2055 cartesian_trajectory_planner_B.v_b[2] * (1.0 -
2056 cartesian_trajectory_planner_B.cth) + cartesian_trajectory_planner_B.cth,
2057 cartesian_trajectory_planner_B.tempR_f);
2058 for (cartesian_trajectory_planner_B.b_kstr_h = 0;
2059 cartesian_trajectory_planner_B.b_kstr_h < 3;
2060 cartesian_trajectory_planner_B.b_kstr_h++) {
2061 cartesian_trajectory_planner_B.loop_ub_k =
2062 cartesian_trajectory_planner_B.b_kstr_h + 1;
2063 cartesian_trajectory_planner_B.R_iz[cartesian_trajectory_planner_B.loop_ub_k
2064 - 1] = cartesian_trajectory_planner_B.tempR_f
2065 [(cartesian_trajectory_planner_B.loop_ub_k - 1) * 3];
2066 cartesian_trajectory_planner_B.loop_ub_k =
2067 cartesian_trajectory_planner_B.b_kstr_h + 1;
2068 cartesian_trajectory_planner_B.R_iz[cartesian_trajectory_planner_B.loop_ub_k
2069 + 2] = cartesian_trajectory_planner_B.tempR_f
2070 [(cartesian_trajectory_planner_B.loop_ub_k - 1) * 3 + 1];
2071 cartesian_trajectory_planner_B.loop_ub_k =
2072 cartesian_trajectory_planner_B.b_kstr_h + 1;
2073 cartesian_trajectory_planner_B.R_iz[cartesian_trajectory_planner_B.loop_ub_k
2074 + 5] = cartesian_trajectory_planner_B.tempR_f
2075 [(cartesian_trajectory_planner_B.loop_ub_k - 1) * 3 + 2];
2076 }
2077
2078 memset(&cartesian_trajectory_planner_B.TJ[0], 0, sizeof(real_T) << 4U);
2079 for (cartesian_trajectory_planner_B.b_kstr_h = 0;
2080 cartesian_trajectory_planner_B.b_kstr_h < 3;
2081 cartesian_trajectory_planner_B.b_kstr_h++) {
2082 cartesian_trajectory_planner_B.loop_ub_k =
2083 cartesian_trajectory_planner_B.b_kstr_h << 2;
2084 cartesian_trajectory_planner_B.TJ[cartesian_trajectory_planner_B.loop_ub_k]
2085 = cartesian_trajectory_planner_B.R_iz[3 *
2086 cartesian_trajectory_planner_B.b_kstr_h];
2087 cartesian_trajectory_planner_B.TJ[cartesian_trajectory_planner_B.loop_ub_k
2088 + 1] = cartesian_trajectory_planner_B.R_iz[3 *
2089 cartesian_trajectory_planner_B.b_kstr_h + 1];
2090 cartesian_trajectory_planner_B.TJ[cartesian_trajectory_planner_B.loop_ub_k
2091 + 2] = cartesian_trajectory_planner_B.R_iz[3 *
2092 cartesian_trajectory_planner_B.b_kstr_h + 2];
2093 }
2094
2095 cartesian_trajectory_planner_B.TJ[15] = 1.0;
2096 break;
2097
2098 default:
2099 rigidBodyJoint_get_JointAxis_a(obj, cartesian_trajectory_planner_B.v_b);
2100 memset(&cartesian_trajectory_planner_B.tempR_f[0], 0, 9U * sizeof(real_T));
2101 cartesian_trajectory_planner_B.tempR_f[0] = 1.0;
2102 cartesian_trajectory_planner_B.tempR_f[4] = 1.0;
2103 cartesian_trajectory_planner_B.tempR_f[8] = 1.0;
2104 for (cartesian_trajectory_planner_B.b_kstr_h = 0;
2105 cartesian_trajectory_planner_B.b_kstr_h < 3;
2106 cartesian_trajectory_planner_B.b_kstr_h++) {
2107 cartesian_trajectory_planner_B.loop_ub_k =
2108 cartesian_trajectory_planner_B.b_kstr_h << 2;
2109 cartesian_trajectory_planner_B.TJ[cartesian_trajectory_planner_B.loop_ub_k]
2110 = cartesian_trajectory_planner_B.tempR_f[3 *
2111 cartesian_trajectory_planner_B.b_kstr_h];
2112 cartesian_trajectory_planner_B.TJ[cartesian_trajectory_planner_B.loop_ub_k
2113 + 1] = cartesian_trajectory_planner_B.tempR_f[3 *
2114 cartesian_trajectory_planner_B.b_kstr_h + 1];
2115 cartesian_trajectory_planner_B.TJ[cartesian_trajectory_planner_B.loop_ub_k
2116 + 2] = cartesian_trajectory_planner_B.tempR_f[3 *
2117 cartesian_trajectory_planner_B.b_kstr_h + 2];
2118 cartesian_trajectory_planner_B.TJ[cartesian_trajectory_planner_B.b_kstr_h
2119 + 12] =
2120 cartesian_trajectory_planner_B.v_b[cartesian_trajectory_planner_B.b_kstr_h]
2121 * q_data[0];
2122 }
2123
2124 cartesian_trajectory_planner_B.TJ[3] = 0.0;
2125 cartesian_trajectory_planner_B.TJ[7] = 0.0;
2126 cartesian_trajectory_planner_B.TJ[11] = 0.0;
2127 cartesian_trajectory_planner_B.TJ[15] = 1.0;
2128 break;
2129 }
2130
2131 for (cartesian_trajectory_planner_B.b_kstr_h = 0;
2132 cartesian_trajectory_planner_B.b_kstr_h < 16;
2133 cartesian_trajectory_planner_B.b_kstr_h++) {
2134 cartesian_trajectory_planner_B.a[cartesian_trajectory_planner_B.b_kstr_h] =
2135 obj->JointToParentTransform[cartesian_trajectory_planner_B.b_kstr_h];
2136 }
2137
2138 for (cartesian_trajectory_planner_B.b_kstr_h = 0;
2139 cartesian_trajectory_planner_B.b_kstr_h < 16;
2140 cartesian_trajectory_planner_B.b_kstr_h++) {
2141 cartesian_trajectory_planner_B.b[cartesian_trajectory_planner_B.b_kstr_h] =
2142 obj->ChildToJointTransform[cartesian_trajectory_planner_B.b_kstr_h];
2143 }
2144
2145 for (cartesian_trajectory_planner_B.b_kstr_h = 0;
2146 cartesian_trajectory_planner_B.b_kstr_h < 4;
2147 cartesian_trajectory_planner_B.b_kstr_h++) {
2148 for (cartesian_trajectory_planner_B.loop_ub_k = 0;
2149 cartesian_trajectory_planner_B.loop_ub_k < 4;
2150 cartesian_trajectory_planner_B.loop_ub_k++) {
2151 cartesian_trajectory_planner_B.a_tmp_tmp =
2152 cartesian_trajectory_planner_B.loop_ub_k << 2;
2153 cartesian_trajectory_planner_B.a_tmp =
2154 cartesian_trajectory_planner_B.b_kstr_h +
2155 cartesian_trajectory_planner_B.a_tmp_tmp;
2156 cartesian_trajectory_planner_B.a_j[cartesian_trajectory_planner_B.a_tmp] =
2157 0.0;
2158 cartesian_trajectory_planner_B.a_j[cartesian_trajectory_planner_B.a_tmp] +=
2159 cartesian_trajectory_planner_B.TJ[cartesian_trajectory_planner_B.a_tmp_tmp]
2160 * cartesian_trajectory_planner_B.a[cartesian_trajectory_planner_B.b_kstr_h];
2161 cartesian_trajectory_planner_B.a_j[cartesian_trajectory_planner_B.a_tmp] +=
2162 cartesian_trajectory_planner_B.TJ[cartesian_trajectory_planner_B.a_tmp_tmp
2163 + 1] *
2164 cartesian_trajectory_planner_B.a[cartesian_trajectory_planner_B.b_kstr_h
2165 + 4];
2166 cartesian_trajectory_planner_B.a_j[cartesian_trajectory_planner_B.a_tmp] +=
2167 cartesian_trajectory_planner_B.TJ[cartesian_trajectory_planner_B.a_tmp_tmp
2168 + 2] *
2169 cartesian_trajectory_planner_B.a[cartesian_trajectory_planner_B.b_kstr_h
2170 + 8];
2171 cartesian_trajectory_planner_B.a_j[cartesian_trajectory_planner_B.a_tmp] +=
2172 cartesian_trajectory_planner_B.TJ[cartesian_trajectory_planner_B.a_tmp_tmp
2173 + 3] *
2174 cartesian_trajectory_planner_B.a[cartesian_trajectory_planner_B.b_kstr_h
2175 + 12];
2176 }
2177
2178 for (cartesian_trajectory_planner_B.loop_ub_k = 0;
2179 cartesian_trajectory_planner_B.loop_ub_k < 4;
2180 cartesian_trajectory_planner_B.loop_ub_k++) {
2181 cartesian_trajectory_planner_B.a_tmp_tmp =
2182 cartesian_trajectory_planner_B.loop_ub_k << 2;
2183 cartesian_trajectory_planner_B.a_tmp =
2184 cartesian_trajectory_planner_B.b_kstr_h +
2185 cartesian_trajectory_planner_B.a_tmp_tmp;
2186 T[cartesian_trajectory_planner_B.a_tmp] = 0.0;
2187 T[cartesian_trajectory_planner_B.a_tmp] +=
2188 cartesian_trajectory_planner_B.b[cartesian_trajectory_planner_B.a_tmp_tmp]
2189 * cartesian_trajectory_planner_B.a_j[cartesian_trajectory_planner_B.b_kstr_h];
2190 T[cartesian_trajectory_planner_B.a_tmp] +=
2191 cartesian_trajectory_planner_B.b[cartesian_trajectory_planner_B.a_tmp_tmp
2192 + 1] *
2193 cartesian_trajectory_planner_B.a_j[cartesian_trajectory_planner_B.b_kstr_h
2194 + 4];
2195 T[cartesian_trajectory_planner_B.a_tmp] +=
2196 cartesian_trajectory_planner_B.b[cartesian_trajectory_planner_B.a_tmp_tmp
2197 + 2] *
2198 cartesian_trajectory_planner_B.a_j[cartesian_trajectory_planner_B.b_kstr_h
2199 + 8];
2200 T[cartesian_trajectory_planner_B.a_tmp] +=
2201 cartesian_trajectory_planner_B.b[cartesian_trajectory_planner_B.a_tmp_tmp
2202 + 3] *
2203 cartesian_trajectory_planner_B.a_j[cartesian_trajectory_planner_B.b_kstr_h
2204 + 12];
2205 }
2206 }
2207}
2208
2209static void rigidBodyJoint_transformBodyToP(const
2210 c_rigidBodyJoint_cartesian__a_T *obj, real_T T[16])
2211{
2212 emxArray_char_T_cartesian_tra_T *switch_expression;
2213 static const char_T tmp[5] = { 'f', 'i', 'x', 'e', 'd' };
2214
2215 static const char_T tmp_0[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
2216
2217 int32_T exitg1;
2218 cartesian_trajec_emxInit_char_T(&switch_expression, 2);
2219 cartesian_trajectory_planner_B.b_kstr_l = switch_expression->size[0] *
2220 switch_expression->size[1];
2221 switch_expression->size[0] = 1;
2222 switch_expression->size[1] = obj->Type->size[1];
2223 cartes_emxEnsureCapacity_char_T(switch_expression,
2224 cartesian_trajectory_planner_B.b_kstr_l);
2225 cartesian_trajectory_planner_B.loop_ub_m = obj->Type->size[0] * obj->
2226 Type->size[1] - 1;
2227 for (cartesian_trajectory_planner_B.b_kstr_l = 0;
2228 cartesian_trajectory_planner_B.b_kstr_l <=
2229 cartesian_trajectory_planner_B.loop_ub_m;
2230 cartesian_trajectory_planner_B.b_kstr_l++) {
2231 switch_expression->data[cartesian_trajectory_planner_B.b_kstr_l] = obj->
2232 Type->data[cartesian_trajectory_planner_B.b_kstr_l];
2233 }
2234
2235 for (cartesian_trajectory_planner_B.b_kstr_l = 0;
2236 cartesian_trajectory_planner_B.b_kstr_l < 5;
2237 cartesian_trajectory_planner_B.b_kstr_l++) {
2238 cartesian_trajectory_planner_B.b_f1s[cartesian_trajectory_planner_B.b_kstr_l]
2239 = tmp[cartesian_trajectory_planner_B.b_kstr_l];
2240 }
2241
2242 cartesian_trajectory_planner_B.b_bool_j = false;
2243 if (switch_expression->size[1] == 5) {
2244 cartesian_trajectory_planner_B.b_kstr_l = 1;
2245 do {
2246 exitg1 = 0;
2247 if (cartesian_trajectory_planner_B.b_kstr_l - 1 < 5) {
2248 cartesian_trajectory_planner_B.loop_ub_m =
2249 cartesian_trajectory_planner_B.b_kstr_l - 1;
2250 if (switch_expression->data[cartesian_trajectory_planner_B.loop_ub_m] !=
2251 cartesian_trajectory_planner_B.b_f1s[cartesian_trajectory_planner_B.loop_ub_m])
2252 {
2253 exitg1 = 1;
2254 } else {
2255 cartesian_trajectory_planner_B.b_kstr_l++;
2256 }
2257 } else {
2258 cartesian_trajectory_planner_B.b_bool_j = true;
2259 exitg1 = 1;
2260 }
2261 } while (exitg1 == 0);
2262 }
2263
2264 if (cartesian_trajectory_planner_B.b_bool_j) {
2265 cartesian_trajectory_planner_B.b_kstr_l = 0;
2266 } else {
2267 for (cartesian_trajectory_planner_B.b_kstr_l = 0;
2268 cartesian_trajectory_planner_B.b_kstr_l < 8;
2269 cartesian_trajectory_planner_B.b_kstr_l++) {
2270 cartesian_trajectory_planner_B.b_p[cartesian_trajectory_planner_B.b_kstr_l]
2271 = tmp_0[cartesian_trajectory_planner_B.b_kstr_l];
2272 }
2273
2274 cartesian_trajectory_planner_B.b_bool_j = false;
2275 if (switch_expression->size[1] == 8) {
2276 cartesian_trajectory_planner_B.b_kstr_l = 1;
2277 do {
2278 exitg1 = 0;
2279 if (cartesian_trajectory_planner_B.b_kstr_l - 1 < 8) {
2280 cartesian_trajectory_planner_B.loop_ub_m =
2281 cartesian_trajectory_planner_B.b_kstr_l - 1;
2282 if (switch_expression->data[cartesian_trajectory_planner_B.loop_ub_m]
2283 !=
2284 cartesian_trajectory_planner_B.b_p[cartesian_trajectory_planner_B.loop_ub_m])
2285 {
2286 exitg1 = 1;
2287 } else {
2288 cartesian_trajectory_planner_B.b_kstr_l++;
2289 }
2290 } else {
2291 cartesian_trajectory_planner_B.b_bool_j = true;
2292 exitg1 = 1;
2293 }
2294 } while (exitg1 == 0);
2295 }
2296
2297 if (cartesian_trajectory_planner_B.b_bool_j) {
2298 cartesian_trajectory_planner_B.b_kstr_l = 1;
2299 } else {
2300 cartesian_trajectory_planner_B.b_kstr_l = -1;
2301 }
2302 }
2303
2304 cartesian_trajec_emxFree_char_T(&switch_expression);
2305 switch (cartesian_trajectory_planner_B.b_kstr_l) {
2306 case 0:
2307 memset(&cartesian_trajectory_planner_B.TJ_d[0], 0, sizeof(real_T) << 4U);
2308 cartesian_trajectory_planner_B.TJ_d[0] = 1.0;
2309 cartesian_trajectory_planner_B.TJ_d[5] = 1.0;
2310 cartesian_trajectory_planner_B.TJ_d[10] = 1.0;
2311 cartesian_trajectory_planner_B.TJ_d[15] = 1.0;
2312 break;
2313
2314 case 1:
2315 rigidBodyJoint_get_JointAxis_a(obj, cartesian_trajectory_planner_B.v_c);
2316 cartesian_trajectory_planner_B.axang_idx_0 =
2317 cartesian_trajectory_planner_B.v_c[0];
2318 cartesian_trajectory_planner_B.axang_idx_1 =
2319 cartesian_trajectory_planner_B.v_c[1];
2320 cartesian_trajectory_planner_B.axang_idx_2 =
2321 cartesian_trajectory_planner_B.v_c[2];
2322 cartesian_trajectory_planner_B.b_j = 1.0 / sqrt
2323 ((cartesian_trajectory_planner_B.axang_idx_0 *
2324 cartesian_trajectory_planner_B.axang_idx_0 +
2325 cartesian_trajectory_planner_B.axang_idx_1 *
2326 cartesian_trajectory_planner_B.axang_idx_1) +
2327 cartesian_trajectory_planner_B.axang_idx_2 *
2328 cartesian_trajectory_planner_B.axang_idx_2);
2329 cartesian_trajectory_planner_B.v_c[0] =
2330 cartesian_trajectory_planner_B.axang_idx_0 *
2331 cartesian_trajectory_planner_B.b_j;
2332 cartesian_trajectory_planner_B.v_c[1] =
2333 cartesian_trajectory_planner_B.axang_idx_1 *
2334 cartesian_trajectory_planner_B.b_j;
2335 cartesian_trajectory_planner_B.v_c[2] =
2336 cartesian_trajectory_planner_B.axang_idx_2 *
2337 cartesian_trajectory_planner_B.b_j;
2338 cartesian_trajectory_planner_B.axang_idx_0 =
2339 cartesian_trajectory_planner_B.v_c[1] *
2340 cartesian_trajectory_planner_B.v_c[0] * 0.0;
2341 cartesian_trajectory_planner_B.axang_idx_1 =
2342 cartesian_trajectory_planner_B.v_c[2] *
2343 cartesian_trajectory_planner_B.v_c[0] * 0.0;
2344 cartesian_trajectory_planner_B.axang_idx_2 =
2345 cartesian_trajectory_planner_B.v_c[2] *
2346 cartesian_trajectory_planner_B.v_c[1] * 0.0;
2347 cartesian_trajectory_planne_cat(cartesian_trajectory_planner_B.v_c[0] *
2348 cartesian_trajectory_planner_B.v_c[0] * 0.0 + 1.0,
2349 cartesian_trajectory_planner_B.axang_idx_0 -
2350 cartesian_trajectory_planner_B.v_c[2] * 0.0,
2351 cartesian_trajectory_planner_B.axang_idx_1 +
2352 cartesian_trajectory_planner_B.v_c[1] * 0.0,
2353 cartesian_trajectory_planner_B.axang_idx_0 +
2354 cartesian_trajectory_planner_B.v_c[2] * 0.0,
2355 cartesian_trajectory_planner_B.v_c[1] *
2356 cartesian_trajectory_planner_B.v_c[1] * 0.0 + 1.0,
2357 cartesian_trajectory_planner_B.axang_idx_2 -
2358 cartesian_trajectory_planner_B.v_c[0] * 0.0,
2359 cartesian_trajectory_planner_B.axang_idx_1 -
2360 cartesian_trajectory_planner_B.v_c[1] * 0.0,
2361 cartesian_trajectory_planner_B.axang_idx_2 +
2362 cartesian_trajectory_planner_B.v_c[0] * 0.0,
2363 cartesian_trajectory_planner_B.v_c[2] *
2364 cartesian_trajectory_planner_B.v_c[2] * 0.0 + 1.0,
2365 cartesian_trajectory_planner_B.tempR_c);
2366 for (cartesian_trajectory_planner_B.b_kstr_l = 0;
2367 cartesian_trajectory_planner_B.b_kstr_l < 3;
2368 cartesian_trajectory_planner_B.b_kstr_l++) {
2369 cartesian_trajectory_planner_B.loop_ub_m =
2370 cartesian_trajectory_planner_B.b_kstr_l + 1;
2371 cartesian_trajectory_planner_B.R_g[cartesian_trajectory_planner_B.loop_ub_m
2372 - 1] = cartesian_trajectory_planner_B.tempR_c
2373 [(cartesian_trajectory_planner_B.loop_ub_m - 1) * 3];
2374 cartesian_trajectory_planner_B.loop_ub_m =
2375 cartesian_trajectory_planner_B.b_kstr_l + 1;
2376 cartesian_trajectory_planner_B.R_g[cartesian_trajectory_planner_B.loop_ub_m
2377 + 2] = cartesian_trajectory_planner_B.tempR_c
2378 [(cartesian_trajectory_planner_B.loop_ub_m - 1) * 3 + 1];
2379 cartesian_trajectory_planner_B.loop_ub_m =
2380 cartesian_trajectory_planner_B.b_kstr_l + 1;
2381 cartesian_trajectory_planner_B.R_g[cartesian_trajectory_planner_B.loop_ub_m
2382 + 5] = cartesian_trajectory_planner_B.tempR_c
2383 [(cartesian_trajectory_planner_B.loop_ub_m - 1) * 3 + 2];
2384 }
2385
2386 memset(&cartesian_trajectory_planner_B.TJ_d[0], 0, sizeof(real_T) << 4U);
2387 for (cartesian_trajectory_planner_B.b_kstr_l = 0;
2388 cartesian_trajectory_planner_B.b_kstr_l < 3;
2389 cartesian_trajectory_planner_B.b_kstr_l++) {
2390 cartesian_trajectory_planner_B.loop_ub_m =
2391 cartesian_trajectory_planner_B.b_kstr_l << 2;
2392 cartesian_trajectory_planner_B.TJ_d[cartesian_trajectory_planner_B.loop_ub_m]
2393 = cartesian_trajectory_planner_B.R_g[3 *
2394 cartesian_trajectory_planner_B.b_kstr_l];
2395 cartesian_trajectory_planner_B.TJ_d[cartesian_trajectory_planner_B.loop_ub_m
2396 + 1] = cartesian_trajectory_planner_B.R_g[3 *
2397 cartesian_trajectory_planner_B.b_kstr_l + 1];
2398 cartesian_trajectory_planner_B.TJ_d[cartesian_trajectory_planner_B.loop_ub_m
2399 + 2] = cartesian_trajectory_planner_B.R_g[3 *
2400 cartesian_trajectory_planner_B.b_kstr_l + 2];
2401 }
2402
2403 cartesian_trajectory_planner_B.TJ_d[15] = 1.0;
2404 break;
2405
2406 default:
2407 rigidBodyJoint_get_JointAxis_a(obj, cartesian_trajectory_planner_B.v_c);
2408 memset(&cartesian_trajectory_planner_B.tempR_c[0], 0, 9U * sizeof(real_T));
2409 cartesian_trajectory_planner_B.tempR_c[0] = 1.0;
2410 cartesian_trajectory_planner_B.tempR_c[4] = 1.0;
2411 cartesian_trajectory_planner_B.tempR_c[8] = 1.0;
2412 for (cartesian_trajectory_planner_B.b_kstr_l = 0;
2413 cartesian_trajectory_planner_B.b_kstr_l < 3;
2414 cartesian_trajectory_planner_B.b_kstr_l++) {
2415 cartesian_trajectory_planner_B.loop_ub_m =
2416 cartesian_trajectory_planner_B.b_kstr_l << 2;
2417 cartesian_trajectory_planner_B.TJ_d[cartesian_trajectory_planner_B.loop_ub_m]
2418 = cartesian_trajectory_planner_B.tempR_c[3 *
2419 cartesian_trajectory_planner_B.b_kstr_l];
2420 cartesian_trajectory_planner_B.TJ_d[cartesian_trajectory_planner_B.loop_ub_m
2421 + 1] = cartesian_trajectory_planner_B.tempR_c[3 *
2422 cartesian_trajectory_planner_B.b_kstr_l + 1];
2423 cartesian_trajectory_planner_B.TJ_d[cartesian_trajectory_planner_B.loop_ub_m
2424 + 2] = cartesian_trajectory_planner_B.tempR_c[3 *
2425 cartesian_trajectory_planner_B.b_kstr_l + 2];
2426 cartesian_trajectory_planner_B.TJ_d[cartesian_trajectory_planner_B.b_kstr_l
2427 + 12] =
2428 cartesian_trajectory_planner_B.v_c[cartesian_trajectory_planner_B.b_kstr_l]
2429 * 0.0;
2430 }
2431
2432 cartesian_trajectory_planner_B.TJ_d[3] = 0.0;
2433 cartesian_trajectory_planner_B.TJ_d[7] = 0.0;
2434 cartesian_trajectory_planner_B.TJ_d[11] = 0.0;
2435 cartesian_trajectory_planner_B.TJ_d[15] = 1.0;
2436 break;
2437 }
2438
2439 for (cartesian_trajectory_planner_B.b_kstr_l = 0;
2440 cartesian_trajectory_planner_B.b_kstr_l < 16;
2441 cartesian_trajectory_planner_B.b_kstr_l++) {
2442 cartesian_trajectory_planner_B.a_g[cartesian_trajectory_planner_B.b_kstr_l] =
2443 obj->JointToParentTransform[cartesian_trajectory_planner_B.b_kstr_l];
2444 }
2445
2446 for (cartesian_trajectory_planner_B.b_kstr_l = 0;
2447 cartesian_trajectory_planner_B.b_kstr_l < 16;
2448 cartesian_trajectory_planner_B.b_kstr_l++) {
2449 cartesian_trajectory_planner_B.b_l[cartesian_trajectory_planner_B.b_kstr_l] =
2450 obj->ChildToJointTransform[cartesian_trajectory_planner_B.b_kstr_l];
2451 }
2452
2453 for (cartesian_trajectory_planner_B.b_kstr_l = 0;
2454 cartesian_trajectory_planner_B.b_kstr_l < 4;
2455 cartesian_trajectory_planner_B.b_kstr_l++) {
2456 for (cartesian_trajectory_planner_B.loop_ub_m = 0;
2457 cartesian_trajectory_planner_B.loop_ub_m < 4;
2458 cartesian_trajectory_planner_B.loop_ub_m++) {
2459 cartesian_trajectory_planner_B.a_tmp_tmp_g =
2460 cartesian_trajectory_planner_B.loop_ub_m << 2;
2461 cartesian_trajectory_planner_B.a_tmp_n =
2462 cartesian_trajectory_planner_B.b_kstr_l +
2463 cartesian_trajectory_planner_B.a_tmp_tmp_g;
2464 cartesian_trajectory_planner_B.a_d[cartesian_trajectory_planner_B.a_tmp_n]
2465 = 0.0;
2466 cartesian_trajectory_planner_B.a_d[cartesian_trajectory_planner_B.a_tmp_n]
2467 +=
2468 cartesian_trajectory_planner_B.TJ_d[cartesian_trajectory_planner_B.a_tmp_tmp_g]
2469 * cartesian_trajectory_planner_B.a_g[cartesian_trajectory_planner_B.b_kstr_l];
2470 cartesian_trajectory_planner_B.a_d[cartesian_trajectory_planner_B.a_tmp_n]
2471 +=
2472 cartesian_trajectory_planner_B.TJ_d[cartesian_trajectory_planner_B.a_tmp_tmp_g
2473 + 1] *
2474 cartesian_trajectory_planner_B.a_g[cartesian_trajectory_planner_B.b_kstr_l
2475 + 4];
2476 cartesian_trajectory_planner_B.a_d[cartesian_trajectory_planner_B.a_tmp_n]
2477 +=
2478 cartesian_trajectory_planner_B.TJ_d[cartesian_trajectory_planner_B.a_tmp_tmp_g
2479 + 2] *
2480 cartesian_trajectory_planner_B.a_g[cartesian_trajectory_planner_B.b_kstr_l
2481 + 8];
2482 cartesian_trajectory_planner_B.a_d[cartesian_trajectory_planner_B.a_tmp_n]
2483 +=
2484 cartesian_trajectory_planner_B.TJ_d[cartesian_trajectory_planner_B.a_tmp_tmp_g
2485 + 3] *
2486 cartesian_trajectory_planner_B.a_g[cartesian_trajectory_planner_B.b_kstr_l
2487 + 12];
2488 }
2489
2490 for (cartesian_trajectory_planner_B.loop_ub_m = 0;
2491 cartesian_trajectory_planner_B.loop_ub_m < 4;
2492 cartesian_trajectory_planner_B.loop_ub_m++) {
2493 cartesian_trajectory_planner_B.a_tmp_tmp_g =
2494 cartesian_trajectory_planner_B.loop_ub_m << 2;
2495 cartesian_trajectory_planner_B.a_tmp_n =
2496 cartesian_trajectory_planner_B.b_kstr_l +
2497 cartesian_trajectory_planner_B.a_tmp_tmp_g;
2498 T[cartesian_trajectory_planner_B.a_tmp_n] = 0.0;
2499 T[cartesian_trajectory_planner_B.a_tmp_n] +=
2500 cartesian_trajectory_planner_B.b_l[cartesian_trajectory_planner_B.a_tmp_tmp_g]
2501 * cartesian_trajectory_planner_B.a_d[cartesian_trajectory_planner_B.b_kstr_l];
2502 T[cartesian_trajectory_planner_B.a_tmp_n] +=
2503 cartesian_trajectory_planner_B.b_l[cartesian_trajectory_planner_B.a_tmp_tmp_g
2504 + 1] *
2505 cartesian_trajectory_planner_B.a_d[cartesian_trajectory_planner_B.b_kstr_l
2506 + 4];
2507 T[cartesian_trajectory_planner_B.a_tmp_n] +=
2508 cartesian_trajectory_planner_B.b_l[cartesian_trajectory_planner_B.a_tmp_tmp_g
2509 + 2] *
2510 cartesian_trajectory_planner_B.a_d[cartesian_trajectory_planner_B.b_kstr_l
2511 + 8];
2512 T[cartesian_trajectory_planner_B.a_tmp_n] +=
2513 cartesian_trajectory_planner_B.b_l[cartesian_trajectory_planner_B.a_tmp_tmp_g
2514 + 3] *
2515 cartesian_trajectory_planner_B.a_d[cartesian_trajectory_planner_B.b_kstr_l
2516 + 12];
2517 }
2518 }
2519}
2520
2521static void RigidBodyTree_efficientFKAndJac(x_robotics_manip_internal_Rig_T *obj,
2522 const real_T qv[6], const emxArray_char_T_cartesian_tra_T *body1Name, real_T
2523 T_data[], int32_T T_size[2], emxArray_real_T_cartesian_tra_T *Jac)
2524{
2525 w_robotics_manip_internal_Rig_T *body1;
2526 w_robotics_manip_internal_Rig_T *body2;
2527 emxArray_real_T_cartesian_tra_T *kinematicPathIndices;
2528 c_rigidBodyJoint_cartesian__a_T *joint;
2529 emxArray_char_T_cartesian_tra_T *body2Name;
2530 emxArray_real_T_cartesian_tra_T *ancestorIndices1;
2531 emxArray_real_T_cartesian_tra_T *ancestorIndices2;
2532 emxArray_real_T_cartesian_tra_T *y;
2533 emxArray_real_T_cartesian_tra_T *b;
2534 w_robotics_manip_internal_Rig_T *body;
2535 emxArray_char_T_cartesian_tra_T *bname;
2536 emxArray_real_T_cartesian_tra_T *ancestorIndices1_0;
2537 emxArray_real_T_cartesian_tra_T *ancestorIndices2_0;
2538 static const char_T tmp[5] = { 'f', 'i', 'x', 'e', 'd' };
2539
2540 boolean_T exitg1;
2541 int32_T exitg2;
2542 cartesian_trajec_emxInit_char_T(&body2Name, 2);
2543 cartesian_trajectory_planner_B.b_kstr = body2Name->size[0] * body2Name->size[1];
2544 body2Name->size[0] = 1;
2545 body2Name->size[1] = obj->Base.NameInternal->size[1];
2546 cartes_emxEnsureCapacity_char_T(body2Name,
2547 cartesian_trajectory_planner_B.b_kstr);
2548 cartesian_trajectory_planner_B.loop_ub_n = obj->Base.NameInternal->size[0] *
2549 obj->Base.NameInternal->size[1] - 1;
2550 for (cartesian_trajectory_planner_B.b_kstr = 0;
2551 cartesian_trajectory_planner_B.b_kstr <=
2552 cartesian_trajectory_planner_B.loop_ub_n;
2553 cartesian_trajectory_planner_B.b_kstr++) {
2554 body2Name->data[cartesian_trajectory_planner_B.b_kstr] =
2555 obj->Base.NameInternal->data[cartesian_trajectory_planner_B.b_kstr];
2556 }
2557
2558 cartesian_trajec_emxInit_char_T(&bname, 2);
2559 cartesian_trajectory_planner_B.bid1 = -1.0;
2560 cartesian_trajectory_planner_B.b_kstr = bname->size[0] * bname->size[1];
2561 bname->size[0] = 1;
2562 bname->size[1] = obj->Base.NameInternal->size[1];
2563 cartes_emxEnsureCapacity_char_T(bname, cartesian_trajectory_planner_B.b_kstr);
2564 cartesian_trajectory_planner_B.loop_ub_n = obj->Base.NameInternal->size[0] *
2565 obj->Base.NameInternal->size[1] - 1;
2566 for (cartesian_trajectory_planner_B.b_kstr = 0;
2567 cartesian_trajectory_planner_B.b_kstr <=
2568 cartesian_trajectory_planner_B.loop_ub_n;
2569 cartesian_trajectory_planner_B.b_kstr++) {
2570 bname->data[cartesian_trajectory_planner_B.b_kstr] = obj->
2571 Base.NameInternal->data[cartesian_trajectory_planner_B.b_kstr];
2572 }
2573
2574 if (cartesian_trajectory_pla_strcmp(bname, body1Name)) {
2575 cartesian_trajectory_planner_B.bid1 = 0.0;
2576 } else {
2577 cartesian_trajectory_planner_B.qidx_idx_1 = obj->NumBodies;
2578 cartesian_trajectory_planner_B.b_i_f = 0;
2579 exitg1 = false;
2580 while ((!exitg1) && (cartesian_trajectory_planner_B.b_i_f <=
2581 static_cast<int32_T>
2582 (cartesian_trajectory_planner_B.qidx_idx_1) - 1)) {
2583 body1 = obj->Bodies[cartesian_trajectory_planner_B.b_i_f];
2584 cartesian_trajectory_planner_B.b_kstr = bname->size[0] * bname->size[1];
2585 bname->size[0] = 1;
2586 bname->size[1] = body1->NameInternal->size[1];
2587 cartes_emxEnsureCapacity_char_T(bname,
2588 cartesian_trajectory_planner_B.b_kstr);
2589 cartesian_trajectory_planner_B.loop_ub_n = body1->NameInternal->size[0] *
2590 body1->NameInternal->size[1] - 1;
2591 for (cartesian_trajectory_planner_B.b_kstr = 0;
2592 cartesian_trajectory_planner_B.b_kstr <=
2593 cartesian_trajectory_planner_B.loop_ub_n;
2594 cartesian_trajectory_planner_B.b_kstr++) {
2595 bname->data[cartesian_trajectory_planner_B.b_kstr] = body1->
2596 NameInternal->data[cartesian_trajectory_planner_B.b_kstr];
2597 }
2598
2599 if (cartesian_trajectory_pla_strcmp(bname, body1Name)) {
2600 cartesian_trajectory_planner_B.bid1 = static_cast<real_T>
2601 (cartesian_trajectory_planner_B.b_i_f) + 1.0;
2602 exitg1 = true;
2603 } else {
2604 cartesian_trajectory_planner_B.b_i_f++;
2605 }
2606 }
2607 }
2608
2609 cartesian_trajectory_planner_B.bid2 = -1.0;
2610 cartesian_trajectory_planner_B.b_kstr = bname->size[0] * bname->size[1];
2611 bname->size[0] = 1;
2612 bname->size[1] = obj->Base.NameInternal->size[1];
2613 cartes_emxEnsureCapacity_char_T(bname, cartesian_trajectory_planner_B.b_kstr);
2614 cartesian_trajectory_planner_B.loop_ub_n = obj->Base.NameInternal->size[0] *
2615 obj->Base.NameInternal->size[1] - 1;
2616 for (cartesian_trajectory_planner_B.b_kstr = 0;
2617 cartesian_trajectory_planner_B.b_kstr <=
2618 cartesian_trajectory_planner_B.loop_ub_n;
2619 cartesian_trajectory_planner_B.b_kstr++) {
2620 bname->data[cartesian_trajectory_planner_B.b_kstr] = obj->
2621 Base.NameInternal->data[cartesian_trajectory_planner_B.b_kstr];
2622 }
2623
2624 if (cartesian_trajectory_pla_strcmp(bname, body2Name)) {
2625 cartesian_trajectory_planner_B.bid2 = 0.0;
2626 } else {
2627 cartesian_trajectory_planner_B.qidx_idx_1 = obj->NumBodies;
2628 cartesian_trajectory_planner_B.b_i_f = 0;
2629 exitg1 = false;
2630 while ((!exitg1) && (cartesian_trajectory_planner_B.b_i_f <=
2631 static_cast<int32_T>
2632 (cartesian_trajectory_planner_B.qidx_idx_1) - 1)) {
2633 body1 = obj->Bodies[cartesian_trajectory_planner_B.b_i_f];
2634 cartesian_trajectory_planner_B.b_kstr = bname->size[0] * bname->size[1];
2635 bname->size[0] = 1;
2636 bname->size[1] = body1->NameInternal->size[1];
2637 cartes_emxEnsureCapacity_char_T(bname,
2638 cartesian_trajectory_planner_B.b_kstr);
2639 cartesian_trajectory_planner_B.loop_ub_n = body1->NameInternal->size[0] *
2640 body1->NameInternal->size[1] - 1;
2641 for (cartesian_trajectory_planner_B.b_kstr = 0;
2642 cartesian_trajectory_planner_B.b_kstr <=
2643 cartesian_trajectory_planner_B.loop_ub_n;
2644 cartesian_trajectory_planner_B.b_kstr++) {
2645 bname->data[cartesian_trajectory_planner_B.b_kstr] = body1->
2646 NameInternal->data[cartesian_trajectory_planner_B.b_kstr];
2647 }
2648
2649 if (cartesian_trajectory_pla_strcmp(bname, body2Name)) {
2650 cartesian_trajectory_planner_B.bid2 = static_cast<real_T>
2651 (cartesian_trajectory_planner_B.b_i_f) + 1.0;
2652 exitg1 = true;
2653 } else {
2654 cartesian_trajectory_planner_B.b_i_f++;
2655 }
2656 }
2657 }
2658
2659 cartesian_trajec_emxFree_char_T(&bname);
2660 if (cartesian_trajectory_planner_B.bid1 == 0.0) {
2661 body1 = &obj->Base;
2662 } else {
2663 body1 = obj->Bodies[static_cast<int32_T>(cartesian_trajectory_planner_B.bid1)
2664 - 1];
2665 }
2666
2667 if (cartesian_trajectory_planner_B.bid2 == 0.0) {
2668 body2 = &obj->Base;
2669 } else {
2670 body2 = obj->Bodies[static_cast<int32_T>(cartesian_trajectory_planner_B.bid2)
2671 - 1];
2672 }
2673
2674 cartesian_trajec_emxInit_real_T(&ancestorIndices1, 2);
2675 body = body1;
2676 cartesian_trajectory_planner_B.b_kstr = ancestorIndices1->size[0] *
2677 ancestorIndices1->size[1];
2678 ancestorIndices1->size[0] = 1;
2679 ancestorIndices1->size[1] = static_cast<int32_T>(obj->NumBodies + 1.0);
2680 cartes_emxEnsureCapacity_real_T(ancestorIndices1,
2681 cartesian_trajectory_planner_B.b_kstr);
2682 cartesian_trajectory_planner_B.loop_ub_n = static_cast<int32_T>(obj->NumBodies
2683 + 1.0) - 1;
2684 for (cartesian_trajectory_planner_B.b_kstr = 0;
2685 cartesian_trajectory_planner_B.b_kstr <=
2686 cartesian_trajectory_planner_B.loop_ub_n;
2687 cartesian_trajectory_planner_B.b_kstr++) {
2688 ancestorIndices1->data[cartesian_trajectory_planner_B.b_kstr] = 0.0;
2689 }
2690
2691 cartesian_trajectory_planner_B.bid1 = 2.0;
2692 ancestorIndices1->data[0] = body1->Index;
2693 while (body->ParentIndex > 0.0) {
2694 body = obj->Bodies[static_cast<int32_T>(body->ParentIndex) - 1];
2695 ancestorIndices1->data[static_cast<int32_T>
2696 (cartesian_trajectory_planner_B.bid1) - 1] = body->Index;
2697 cartesian_trajectory_planner_B.bid1++;
2698 }
2699
2700 if (body->Index > 0.0) {
2701 ancestorIndices1->data[static_cast<int32_T>
2702 (cartesian_trajectory_planner_B.bid1) - 1] = body->ParentIndex;
2703 cartesian_trajectory_planner_B.bid1++;
2704 }
2705
2706 cartesian_trajec_emxInit_real_T(&ancestorIndices1_0, 2);
2707 cartesian_trajectory_planner_B.loop_ub_n = static_cast<int32_T>
2708 (cartesian_trajectory_planner_B.bid1 - 1.0);
2709 cartesian_trajectory_planner_B.b_kstr = ancestorIndices1_0->size[0] *
2710 ancestorIndices1_0->size[1];
2711 ancestorIndices1_0->size[0] = 1;
2712 ancestorIndices1_0->size[1] = cartesian_trajectory_planner_B.loop_ub_n;
2713 cartes_emxEnsureCapacity_real_T(ancestorIndices1_0,
2714 cartesian_trajectory_planner_B.b_kstr);
2715 for (cartesian_trajectory_planner_B.b_kstr = 0;
2716 cartesian_trajectory_planner_B.b_kstr <
2717 cartesian_trajectory_planner_B.loop_ub_n;
2718 cartesian_trajectory_planner_B.b_kstr++) {
2719 ancestorIndices1_0->data[cartesian_trajectory_planner_B.b_kstr] =
2720 ancestorIndices1->data[cartesian_trajectory_planner_B.b_kstr];
2721 }
2722
2723 cartesian_trajectory_planner_B.b_kstr = ancestorIndices1->size[0] *
2724 ancestorIndices1->size[1];
2725 ancestorIndices1->size[0] = 1;
2726 ancestorIndices1->size[1] = ancestorIndices1_0->size[1];
2727 cartes_emxEnsureCapacity_real_T(ancestorIndices1,
2728 cartesian_trajectory_planner_B.b_kstr);
2729 cartesian_trajectory_planner_B.loop_ub_n = ancestorIndices1_0->size[0] *
2730 ancestorIndices1_0->size[1];
2731 for (cartesian_trajectory_planner_B.b_kstr = 0;
2732 cartesian_trajectory_planner_B.b_kstr <
2733 cartesian_trajectory_planner_B.loop_ub_n;
2734 cartesian_trajectory_planner_B.b_kstr++) {
2735 ancestorIndices1->data[cartesian_trajectory_planner_B.b_kstr] =
2736 ancestorIndices1_0->data[cartesian_trajectory_planner_B.b_kstr];
2737 }
2738
2739 cartesian_trajec_emxFree_real_T(&ancestorIndices1_0);
2740 cartesian_trajec_emxInit_real_T(&ancestorIndices2, 2);
2741 body = body2;
2742 cartesian_trajectory_planner_B.b_kstr = ancestorIndices2->size[0] *
2743 ancestorIndices2->size[1];
2744 ancestorIndices2->size[0] = 1;
2745 ancestorIndices2->size[1] = static_cast<int32_T>(obj->NumBodies + 1.0);
2746 cartes_emxEnsureCapacity_real_T(ancestorIndices2,
2747 cartesian_trajectory_planner_B.b_kstr);
2748 cartesian_trajectory_planner_B.loop_ub_n = static_cast<int32_T>(obj->NumBodies
2749 + 1.0) - 1;
2750 for (cartesian_trajectory_planner_B.b_kstr = 0;
2751 cartesian_trajectory_planner_B.b_kstr <=
2752 cartesian_trajectory_planner_B.loop_ub_n;
2753 cartesian_trajectory_planner_B.b_kstr++) {
2754 ancestorIndices2->data[cartesian_trajectory_planner_B.b_kstr] = 0.0;
2755 }
2756
2757 cartesian_trajectory_planner_B.bid1 = 2.0;
2758 ancestorIndices2->data[0] = body2->Index;
2759 while (body->ParentIndex > 0.0) {
2760 body = obj->Bodies[static_cast<int32_T>(body->ParentIndex) - 1];
2761 ancestorIndices2->data[static_cast<int32_T>
2762 (cartesian_trajectory_planner_B.bid1) - 1] = body->Index;
2763 cartesian_trajectory_planner_B.bid1++;
2764 }
2765
2766 if (body->Index > 0.0) {
2767 ancestorIndices2->data[static_cast<int32_T>
2768 (cartesian_trajectory_planner_B.bid1) - 1] = body->ParentIndex;
2769 cartesian_trajectory_planner_B.bid1++;
2770 }
2771
2772 cartesian_trajec_emxInit_real_T(&ancestorIndices2_0, 2);
2773 cartesian_trajectory_planner_B.loop_ub_n = static_cast<int32_T>
2774 (cartesian_trajectory_planner_B.bid1 - 1.0);
2775 cartesian_trajectory_planner_B.b_kstr = ancestorIndices2_0->size[0] *
2776 ancestorIndices2_0->size[1];
2777 ancestorIndices2_0->size[0] = 1;
2778 ancestorIndices2_0->size[1] = cartesian_trajectory_planner_B.loop_ub_n;
2779 cartes_emxEnsureCapacity_real_T(ancestorIndices2_0,
2780 cartesian_trajectory_planner_B.b_kstr);
2781 for (cartesian_trajectory_planner_B.b_kstr = 0;
2782 cartesian_trajectory_planner_B.b_kstr <
2783 cartesian_trajectory_planner_B.loop_ub_n;
2784 cartesian_trajectory_planner_B.b_kstr++) {
2785 ancestorIndices2_0->data[cartesian_trajectory_planner_B.b_kstr] =
2786 ancestorIndices2->data[cartesian_trajectory_planner_B.b_kstr];
2787 }
2788
2789 cartesian_trajectory_planner_B.b_kstr = ancestorIndices2->size[0] *
2790 ancestorIndices2->size[1];
2791 ancestorIndices2->size[0] = 1;
2792 ancestorIndices2->size[1] = ancestorIndices2_0->size[1];
2793 cartes_emxEnsureCapacity_real_T(ancestorIndices2,
2794 cartesian_trajectory_planner_B.b_kstr);
2795 cartesian_trajectory_planner_B.loop_ub_n = ancestorIndices2_0->size[0] *
2796 ancestorIndices2_0->size[1];
2797 for (cartesian_trajectory_planner_B.b_kstr = 0;
2798 cartesian_trajectory_planner_B.b_kstr <
2799 cartesian_trajectory_planner_B.loop_ub_n;
2800 cartesian_trajectory_planner_B.b_kstr++) {
2801 ancestorIndices2->data[cartesian_trajectory_planner_B.b_kstr] =
2802 ancestorIndices2_0->data[cartesian_trajectory_planner_B.b_kstr];
2803 }
2804
2805 cartesian_trajec_emxFree_real_T(&ancestorIndices2_0);
2806 cartesian_trajectory_planner_B.bid1 = ancestorIndices1->size[1];
2807 cartesian_trajectory_planner_B.qidx_idx_1 = ancestorIndices2->size[1];
2808 if (cartesian_trajectory_planner_B.bid1 <
2809 cartesian_trajectory_planner_B.qidx_idx_1) {
2810 cartesian_trajectory_planner_B.qidx_idx_1 =
2811 cartesian_trajectory_planner_B.bid1;
2812 }
2813
2814 cartesian_trajectory_planner_B.bid1 = static_cast<int32_T>
2815 (cartesian_trajectory_planner_B.qidx_idx_1);
2816 cartesian_trajectory_planner_B.b_i_f = 0;
2817 exitg1 = false;
2818 while ((!exitg1) && (cartesian_trajectory_planner_B.b_i_f <=
2819 static_cast<int32_T>
2820 (cartesian_trajectory_planner_B.qidx_idx_1) - 2)) {
2821 if (ancestorIndices1->data[static_cast<int32_T>(static_cast<real_T>
2822 (ancestorIndices1->size[1]) - (static_cast<real_T>
2823 (cartesian_trajectory_planner_B.b_i_f) + 1.0)) - 1] !=
2824 ancestorIndices2->data[static_cast<int32_T>(static_cast<real_T>
2825 (ancestorIndices2->size[1]) - (static_cast<real_T>
2826 (cartesian_trajectory_planner_B.b_i_f) + 1.0)) - 1]) {
2827 cartesian_trajectory_planner_B.bid1 = static_cast<real_T>
2828 (cartesian_trajectory_planner_B.b_i_f) + 1.0;
2829 exitg1 = true;
2830 } else {
2831 cartesian_trajectory_planner_B.b_i_f++;
2832 }
2833 }
2834
2835 cartesian_trajectory_planner_B.qidx_idx_1 = static_cast<real_T>
2836 (ancestorIndices1->size[1]) - cartesian_trajectory_planner_B.bid1;
2837 if (1.0 > cartesian_trajectory_planner_B.qidx_idx_1) {
2838 cartesian_trajectory_planner_B.b_i_f = -1;
2839 } else {
2840 cartesian_trajectory_planner_B.b_i_f = static_cast<int32_T>
2841 (cartesian_trajectory_planner_B.qidx_idx_1) - 1;
2842 }
2843
2844 cartesian_trajectory_planner_B.qidx_idx_1 = static_cast<real_T>
2845 (ancestorIndices2->size[1]) - cartesian_trajectory_planner_B.bid1;
2846 if (1.0 > cartesian_trajectory_planner_B.qidx_idx_1) {
2847 cartesian_trajectory_planner_B.j_i = 0;
2848 cartesian_trajectory_planner_B.jointSign = 1;
2849 cartesian_trajectory_planner_B.g = -1;
2850 } else {
2851 cartesian_trajectory_planner_B.j_i = static_cast<int32_T>
2852 (cartesian_trajectory_planner_B.qidx_idx_1) - 1;
2853 cartesian_trajectory_planner_B.jointSign = -1;
2854 cartesian_trajectory_planner_B.g = 0;
2855 }
2856
2857 cartesian_trajec_emxInit_real_T(&kinematicPathIndices, 2);
2858 cartesian_trajectory_planner_B.b_kstr = kinematicPathIndices->size[0] *
2859 kinematicPathIndices->size[1];
2860 kinematicPathIndices->size[0] = 1;
2861 cartesian_trajectory_planner_B.loop_ub_n = div_s32_floor
2862 (cartesian_trajectory_planner_B.g - cartesian_trajectory_planner_B.j_i,
2863 cartesian_trajectory_planner_B.jointSign);
2864 kinematicPathIndices->size[1] = (cartesian_trajectory_planner_B.loop_ub_n +
2865 cartesian_trajectory_planner_B.b_i_f) + 3;
2866 cartes_emxEnsureCapacity_real_T(kinematicPathIndices,
2867 cartesian_trajectory_planner_B.b_kstr);
2868 for (cartesian_trajectory_planner_B.b_kstr = 0;
2869 cartesian_trajectory_planner_B.b_kstr <=
2870 cartesian_trajectory_planner_B.b_i_f;
2871 cartesian_trajectory_planner_B.b_kstr++) {
2872 kinematicPathIndices->data[cartesian_trajectory_planner_B.b_kstr] =
2873 ancestorIndices1->data[cartesian_trajectory_planner_B.b_kstr];
2874 }
2875
2876 kinematicPathIndices->data[cartesian_trajectory_planner_B.b_i_f + 1] =
2877 ancestorIndices1->data[static_cast<int32_T>((static_cast<real_T>
2878 (ancestorIndices1->size[1]) - cartesian_trajectory_planner_B.bid1) + 1.0) -
2879 1];
2880 cartesian_trajec_emxFree_real_T(&ancestorIndices1);
2881 for (cartesian_trajectory_planner_B.b_kstr = 0;
2882 cartesian_trajectory_planner_B.b_kstr <=
2883 cartesian_trajectory_planner_B.loop_ub_n;
2884 cartesian_trajectory_planner_B.b_kstr++) {
2885 kinematicPathIndices->data[(cartesian_trajectory_planner_B.b_kstr +
2886 cartesian_trajectory_planner_B.b_i_f) + 2] = ancestorIndices2->
2887 data[cartesian_trajectory_planner_B.jointSign *
2888 cartesian_trajectory_planner_B.b_kstr + cartesian_trajectory_planner_B.j_i];
2889 }
2890
2891 cartesian_trajec_emxFree_real_T(&ancestorIndices2);
2892 memset(&cartesian_trajectory_planner_B.T1[0], 0, sizeof(real_T) << 4U);
2893 cartesian_trajectory_planner_B.T1[0] = 1.0;
2894 cartesian_trajectory_planner_B.T1[5] = 1.0;
2895 cartesian_trajectory_planner_B.T1[10] = 1.0;
2896 cartesian_trajectory_planner_B.T1[15] = 1.0;
2897 cartesian_trajectory_planner_B.b_kstr = Jac->size[0] * Jac->size[1];
2898 Jac->size[0] = 6;
2899 Jac->size[1] = static_cast<int32_T>(obj->PositionNumber);
2900 cartes_emxEnsureCapacity_real_T(Jac, cartesian_trajectory_planner_B.b_kstr);
2901 cartesian_trajectory_planner_B.loop_ub_n = 6 * static_cast<int32_T>
2902 (obj->PositionNumber) - 1;
2903 for (cartesian_trajectory_planner_B.b_kstr = 0;
2904 cartesian_trajectory_planner_B.b_kstr <=
2905 cartesian_trajectory_planner_B.loop_ub_n;
2906 cartesian_trajectory_planner_B.b_kstr++) {
2907 Jac->data[cartesian_trajectory_planner_B.b_kstr] = 0.0;
2908 }
2909
2910 cartesian_trajectory_planner_B.j_i = kinematicPathIndices->size[1] - 2;
2911 cartesian_trajec_emxInit_real_T(&y, 2);
2912 cartesian_trajec_emxInit_real_T(&b, 2);
2913 if (0 <= cartesian_trajectory_planner_B.j_i) {
2914 for (cartesian_trajectory_planner_B.b_kstr = 0;
2915 cartesian_trajectory_planner_B.b_kstr < 5;
2916 cartesian_trajectory_planner_B.b_kstr++) {
2917 cartesian_trajectory_planner_B.b_dv[cartesian_trajectory_planner_B.b_kstr]
2918 = tmp[cartesian_trajectory_planner_B.b_kstr];
2919 }
2920 }
2921
2922 for (cartesian_trajectory_planner_B.b_i_f = 0;
2923 cartesian_trajectory_planner_B.b_i_f <=
2924 cartesian_trajectory_planner_B.j_i; cartesian_trajectory_planner_B.b_i_f
2925 ++) {
2926 if (kinematicPathIndices->data[cartesian_trajectory_planner_B.b_i_f] != 0.0)
2927 {
2928 body1 = obj->Bodies[static_cast<int32_T>(kinematicPathIndices->
2929 data[cartesian_trajectory_planner_B.b_i_f]) - 1];
2930 } else {
2931 body1 = &obj->Base;
2932 }
2933
2934 cartesian_trajectory_planner_B.b_kstr = static_cast<int32_T>
2935 ((static_cast<real_T>(cartesian_trajectory_planner_B.b_i_f) + 1.0) + 1.0)
2936 - 1;
2937 if (kinematicPathIndices->data[cartesian_trajectory_planner_B.b_kstr] != 0.0)
2938 {
2939 body2 = obj->Bodies[static_cast<int32_T>(kinematicPathIndices->
2940 data[cartesian_trajectory_planner_B.b_kstr]) - 1];
2941 } else {
2942 body2 = &obj->Base;
2943 }
2944
2945 cartesian_trajectory_planner_B.nextBodyIsParent = (body2->Index ==
2946 body1->ParentIndex);
2947 if (cartesian_trajectory_planner_B.nextBodyIsParent) {
2948 body2 = body1;
2949 cartesian_trajectory_planner_B.jointSign = 1;
2950 } else {
2951 cartesian_trajectory_planner_B.jointSign = -1;
2952 }
2953
2954 joint = body2->JointInternal;
2955 cartesian_trajectory_planner_B.b_kstr = body2Name->size[0] * body2Name->
2956 size[1];
2957 body2Name->size[0] = 1;
2958 body2Name->size[1] = joint->Type->size[1];
2959 cartes_emxEnsureCapacity_char_T(body2Name,
2960 cartesian_trajectory_planner_B.b_kstr);
2961 cartesian_trajectory_planner_B.loop_ub_n = joint->Type->size[0] *
2962 joint->Type->size[1] - 1;
2963 for (cartesian_trajectory_planner_B.b_kstr = 0;
2964 cartesian_trajectory_planner_B.b_kstr <=
2965 cartesian_trajectory_planner_B.loop_ub_n;
2966 cartesian_trajectory_planner_B.b_kstr++) {
2967 body2Name->data[cartesian_trajectory_planner_B.b_kstr] = joint->Type->
2968 data[cartesian_trajectory_planner_B.b_kstr];
2969 }
2970
2971 cartesian_trajectory_planner_B.b_bool = false;
2972 if (body2Name->size[1] == 5) {
2973 cartesian_trajectory_planner_B.b_kstr = 1;
2974 do {
2975 exitg2 = 0;
2976 if (cartesian_trajectory_planner_B.b_kstr - 1 < 5) {
2977 cartesian_trajectory_planner_B.g =
2978 cartesian_trajectory_planner_B.b_kstr - 1;
2979 if (body2Name->data[cartesian_trajectory_planner_B.g] !=
2980 cartesian_trajectory_planner_B.b_dv[cartesian_trajectory_planner_B.g])
2981 {
2982 exitg2 = 1;
2983 } else {
2984 cartesian_trajectory_planner_B.b_kstr++;
2985 }
2986 } else {
2987 cartesian_trajectory_planner_B.b_bool = true;
2988 exitg2 = 1;
2989 }
2990 } while (exitg2 == 0);
2991 }
2992
2993 if (cartesian_trajectory_planner_B.b_bool) {
2994 rigidBodyJoint_transformBodyToP(joint, cartesian_trajectory_planner_B.Tc2p);
2995 } else {
2996 cartesian_trajectory_planner_B.b_kstr = static_cast<int32_T>(body2->Index);
2997 cartesian_trajectory_planner_B.bid1 = obj->
2998 PositionDoFMap[cartesian_trajectory_planner_B.b_kstr - 1];
2999 cartesian_trajectory_planner_B.qidx_idx_1 = obj->
3000 PositionDoFMap[cartesian_trajectory_planner_B.b_kstr + 7];
3001 if (cartesian_trajectory_planner_B.bid1 >
3002 cartesian_trajectory_planner_B.qidx_idx_1) {
3003 cartesian_trajectory_planner_B.g = 0;
3004 cartesian_trajectory_planner_B.b_kstr = -1;
3005 } else {
3006 cartesian_trajectory_planner_B.g = static_cast<int32_T>
3007 (cartesian_trajectory_planner_B.bid1) - 1;
3008 cartesian_trajectory_planner_B.b_kstr = static_cast<int32_T>
3009 (cartesian_trajectory_planner_B.qidx_idx_1) - 1;
3010 }
3011
3012 cartesian_trajectory_planner_B.loop_ub_n =
3013 cartesian_trajectory_planner_B.b_kstr - cartesian_trajectory_planner_B.g;
3014 cartesian_trajectory_planner_B.qv_size =
3015 cartesian_trajectory_planner_B.loop_ub_n + 1;
3016 for (cartesian_trajectory_planner_B.b_kstr = 0;
3017 cartesian_trajectory_planner_B.b_kstr <=
3018 cartesian_trajectory_planner_B.loop_ub_n;
3019 cartesian_trajectory_planner_B.b_kstr++) {
3020 cartesian_trajectory_planner_B.qv_data[cartesian_trajectory_planner_B.b_kstr]
3021 = qv[cartesian_trajectory_planner_B.g +
3022 cartesian_trajectory_planner_B.b_kstr];
3023 }
3024
3025 rigidBodyJoint_transformBodyT_a(joint,
3026 cartesian_trajectory_planner_B.qv_data,
3027 &cartesian_trajectory_planner_B.qv_size,
3028 cartesian_trajectory_planner_B.Tc2p);
3029 cartesian_trajectory_planner_B.b_kstr = static_cast<int32_T>(body2->Index);
3030 cartesian_trajectory_planner_B.bid1 = obj->
3031 VelocityDoFMap[cartesian_trajectory_planner_B.b_kstr - 1];
3032 cartesian_trajectory_planner_B.qidx_idx_1 = obj->
3033 VelocityDoFMap[cartesian_trajectory_planner_B.b_kstr + 7];
3034 if (cartesian_trajectory_planner_B.nextBodyIsParent) {
3035 for (cartesian_trajectory_planner_B.b_kstr = 0;
3036 cartesian_trajectory_planner_B.b_kstr < 16;
3037 cartesian_trajectory_planner_B.b_kstr++) {
3038 cartesian_trajectory_planner_B.Tj[cartesian_trajectory_planner_B.b_kstr]
3039 = joint->ChildToJointTransform[cartesian_trajectory_planner_B.b_kstr];
3040 }
3041 } else {
3042 for (cartesian_trajectory_planner_B.b_kstr = 0;
3043 cartesian_trajectory_planner_B.b_kstr < 16;
3044 cartesian_trajectory_planner_B.b_kstr++) {
3045 cartesian_trajectory_planner_B.T1j[cartesian_trajectory_planner_B.b_kstr]
3046 = joint->
3047 JointToParentTransform[cartesian_trajectory_planner_B.b_kstr];
3048 }
3049
3050 for (cartesian_trajectory_planner_B.b_kstr = 0;
3051 cartesian_trajectory_planner_B.b_kstr < 3;
3052 cartesian_trajectory_planner_B.b_kstr++) {
3053 cartesian_trajectory_planner_B.R_i[3 *
3054 cartesian_trajectory_planner_B.b_kstr] =
3055 cartesian_trajectory_planner_B.T1j[cartesian_trajectory_planner_B.b_kstr];
3056 cartesian_trajectory_planner_B.R_i[3 *
3057 cartesian_trajectory_planner_B.b_kstr + 1] =
3058 cartesian_trajectory_planner_B.T1j[cartesian_trajectory_planner_B.b_kstr
3059 + 4];
3060 cartesian_trajectory_planner_B.R_i[3 *
3061 cartesian_trajectory_planner_B.b_kstr + 2] =
3062 cartesian_trajectory_planner_B.T1j[cartesian_trajectory_planner_B.b_kstr
3063 + 8];
3064 }
3065
3066 for (cartesian_trajectory_planner_B.b_kstr = 0;
3067 cartesian_trajectory_planner_B.b_kstr < 9;
3068 cartesian_trajectory_planner_B.b_kstr++) {
3069 cartesian_trajectory_planner_B.R_f[cartesian_trajectory_planner_B.b_kstr]
3070 =
3071 -cartesian_trajectory_planner_B.R_i[cartesian_trajectory_planner_B.b_kstr];
3072 }
3073
3074 for (cartesian_trajectory_planner_B.b_kstr = 0;
3075 cartesian_trajectory_planner_B.b_kstr < 3;
3076 cartesian_trajectory_planner_B.b_kstr++) {
3077 cartesian_trajectory_planner_B.g =
3078 cartesian_trajectory_planner_B.b_kstr << 2;
3079 cartesian_trajectory_planner_B.Tj[cartesian_trajectory_planner_B.g] =
3080 cartesian_trajectory_planner_B.R_i[3 *
3081 cartesian_trajectory_planner_B.b_kstr];
3082 cartesian_trajectory_planner_B.Tj[cartesian_trajectory_planner_B.g + 1]
3083 = cartesian_trajectory_planner_B.R_i[3 *
3084 cartesian_trajectory_planner_B.b_kstr + 1];
3085 cartesian_trajectory_planner_B.Tj[cartesian_trajectory_planner_B.g + 2]
3086 = cartesian_trajectory_planner_B.R_i[3 *
3087 cartesian_trajectory_planner_B.b_kstr + 2];
3088 cartesian_trajectory_planner_B.Tj[cartesian_trajectory_planner_B.b_kstr
3089 + 12] =
3090 cartesian_trajectory_planner_B.R_f[cartesian_trajectory_planner_B.b_kstr
3091 + 6] * cartesian_trajectory_planner_B.T1j[14] +
3092 (cartesian_trajectory_planner_B.R_f[cartesian_trajectory_planner_B.b_kstr
3093 + 3] * cartesian_trajectory_planner_B.T1j[13] +
3094 cartesian_trajectory_planner_B.R_f[cartesian_trajectory_planner_B.b_kstr]
3095 * cartesian_trajectory_planner_B.T1j[12]);
3096 }
3097
3098 cartesian_trajectory_planner_B.Tj[3] = 0.0;
3099 cartesian_trajectory_planner_B.Tj[7] = 0.0;
3100 cartesian_trajectory_planner_B.Tj[11] = 0.0;
3101 cartesian_trajectory_planner_B.Tj[15] = 1.0;
3102 }
3103
3104 for (cartesian_trajectory_planner_B.b_kstr = 0;
3105 cartesian_trajectory_planner_B.b_kstr < 4;
3106 cartesian_trajectory_planner_B.b_kstr++) {
3107 for (cartesian_trajectory_planner_B.g = 0;
3108 cartesian_trajectory_planner_B.g < 4;
3109 cartesian_trajectory_planner_B.g++) {
3110 cartesian_trajectory_planner_B.loop_ub_n =
3111 cartesian_trajectory_planner_B.g << 2;
3112 cartesian_trajectory_planner_B.n_c =
3113 cartesian_trajectory_planner_B.b_kstr +
3114 cartesian_trajectory_planner_B.loop_ub_n;
3115 cartesian_trajectory_planner_B.T1j[cartesian_trajectory_planner_B.n_c]
3116 = 0.0;
3117 cartesian_trajectory_planner_B.T1j[cartesian_trajectory_planner_B.n_c]
3118 +=
3119 cartesian_trajectory_planner_B.T1[cartesian_trajectory_planner_B.loop_ub_n]
3120 * cartesian_trajectory_planner_B.Tj[cartesian_trajectory_planner_B.b_kstr];
3121 cartesian_trajectory_planner_B.T1j[cartesian_trajectory_planner_B.n_c]
3122 +=
3123 cartesian_trajectory_planner_B.T1[cartesian_trajectory_planner_B.loop_ub_n
3124 + 1] *
3125 cartesian_trajectory_planner_B.Tj[cartesian_trajectory_planner_B.b_kstr
3126 + 4];
3127 cartesian_trajectory_planner_B.T1j[cartesian_trajectory_planner_B.n_c]
3128 +=
3129 cartesian_trajectory_planner_B.T1[cartesian_trajectory_planner_B.loop_ub_n
3130 + 2] *
3131 cartesian_trajectory_planner_B.Tj[cartesian_trajectory_planner_B.b_kstr
3132 + 8];
3133 cartesian_trajectory_planner_B.T1j[cartesian_trajectory_planner_B.n_c]
3134 +=
3135 cartesian_trajectory_planner_B.T1[cartesian_trajectory_planner_B.loop_ub_n
3136 + 3] *
3137 cartesian_trajectory_planner_B.Tj[cartesian_trajectory_planner_B.b_kstr
3138 + 12];
3139 }
3140 }
3141
3142 for (cartesian_trajectory_planner_B.b_kstr = 0;
3143 cartesian_trajectory_planner_B.b_kstr < 3;
3144 cartesian_trajectory_planner_B.b_kstr++) {
3145 cartesian_trajectory_planner_B.R_i[3 *
3146 cartesian_trajectory_planner_B.b_kstr] =
3147 cartesian_trajectory_planner_B.T1j[cartesian_trajectory_planner_B.b_kstr];
3148 cartesian_trajectory_planner_B.R_i[3 *
3149 cartesian_trajectory_planner_B.b_kstr + 1] =
3150 cartesian_trajectory_planner_B.T1j[cartesian_trajectory_planner_B.b_kstr
3151 + 4];
3152 cartesian_trajectory_planner_B.R_i[3 *
3153 cartesian_trajectory_planner_B.b_kstr + 2] =
3154 cartesian_trajectory_planner_B.T1j[cartesian_trajectory_planner_B.b_kstr
3155 + 8];
3156 }
3157
3158 for (cartesian_trajectory_planner_B.b_kstr = 0;
3159 cartesian_trajectory_planner_B.b_kstr < 9;
3160 cartesian_trajectory_planner_B.b_kstr++) {
3161 cartesian_trajectory_planner_B.R_f[cartesian_trajectory_planner_B.b_kstr]
3162 =
3163 -cartesian_trajectory_planner_B.R_i[cartesian_trajectory_planner_B.b_kstr];
3164 }
3165
3166 for (cartesian_trajectory_planner_B.b_kstr = 0;
3167 cartesian_trajectory_planner_B.b_kstr < 3;
3168 cartesian_trajectory_planner_B.b_kstr++) {
3169 cartesian_trajectory_planner_B.g = cartesian_trajectory_planner_B.b_kstr
3170 << 2;
3171 cartesian_trajectory_planner_B.Tj[cartesian_trajectory_planner_B.g] =
3172 cartesian_trajectory_planner_B.R_i[3 *
3173 cartesian_trajectory_planner_B.b_kstr];
3174 cartesian_trajectory_planner_B.Tj[cartesian_trajectory_planner_B.g + 1] =
3175 cartesian_trajectory_planner_B.R_i[3 *
3176 cartesian_trajectory_planner_B.b_kstr + 1];
3177 cartesian_trajectory_planner_B.Tj[cartesian_trajectory_planner_B.g + 2] =
3178 cartesian_trajectory_planner_B.R_i[3 *
3179 cartesian_trajectory_planner_B.b_kstr + 2];
3180 cartesian_trajectory_planner_B.Tj[cartesian_trajectory_planner_B.b_kstr
3181 + 12] =
3182 cartesian_trajectory_planner_B.R_f[cartesian_trajectory_planner_B.b_kstr
3183 + 6] * cartesian_trajectory_planner_B.T1j[14] +
3184 (cartesian_trajectory_planner_B.R_f[cartesian_trajectory_planner_B.b_kstr
3185 + 3] * cartesian_trajectory_planner_B.T1j[13] +
3186 cartesian_trajectory_planner_B.R_f[cartesian_trajectory_planner_B.b_kstr]
3187 * cartesian_trajectory_planner_B.T1j[12]);
3188 }
3189
3190 cartesian_trajectory_planner_B.Tj[3] = 0.0;
3191 cartesian_trajectory_planner_B.Tj[7] = 0.0;
3192 cartesian_trajectory_planner_B.Tj[11] = 0.0;
3193 cartesian_trajectory_planner_B.Tj[15] = 1.0;
3194 cartesian_trajectory_planner_B.R_i[0] = 0.0;
3195 cartesian_trajectory_planner_B.R_i[3] =
3196 -cartesian_trajectory_planner_B.Tj[14];
3197 cartesian_trajectory_planner_B.R_i[6] = cartesian_trajectory_planner_B.Tj
3198 [13];
3199 cartesian_trajectory_planner_B.R_i[1] = cartesian_trajectory_planner_B.Tj
3200 [14];
3201 cartesian_trajectory_planner_B.R_i[4] = 0.0;
3202 cartesian_trajectory_planner_B.R_i[7] =
3203 -cartesian_trajectory_planner_B.Tj[12];
3204 cartesian_trajectory_planner_B.R_i[2] =
3205 -cartesian_trajectory_planner_B.Tj[13];
3206 cartesian_trajectory_planner_B.R_i[5] = cartesian_trajectory_planner_B.Tj
3207 [12];
3208 cartesian_trajectory_planner_B.R_i[8] = 0.0;
3209 for (cartesian_trajectory_planner_B.b_kstr = 0;
3210 cartesian_trajectory_planner_B.b_kstr < 3;
3211 cartesian_trajectory_planner_B.b_kstr++) {
3212 for (cartesian_trajectory_planner_B.g = 0;
3213 cartesian_trajectory_planner_B.g < 3;
3214 cartesian_trajectory_planner_B.g++) {
3215 cartesian_trajectory_planner_B.loop_ub_n =
3216 cartesian_trajectory_planner_B.b_kstr + 3 *
3217 cartesian_trajectory_planner_B.g;
3218 cartesian_trajectory_planner_B.R_f[cartesian_trajectory_planner_B.loop_ub_n]
3219 = 0.0;
3220 cartesian_trajectory_planner_B.n_c = cartesian_trajectory_planner_B.g <<
3221 2;
3222 cartesian_trajectory_planner_B.R_f[cartesian_trajectory_planner_B.loop_ub_n]
3223 +=
3224 cartesian_trajectory_planner_B.Tj[cartesian_trajectory_planner_B.n_c]
3225 * cartesian_trajectory_planner_B.R_i[cartesian_trajectory_planner_B.b_kstr];
3226 cartesian_trajectory_planner_B.R_f[cartesian_trajectory_planner_B.loop_ub_n]
3227 +=
3228 cartesian_trajectory_planner_B.Tj[cartesian_trajectory_planner_B.n_c
3229 + 1] *
3230 cartesian_trajectory_planner_B.R_i[cartesian_trajectory_planner_B.b_kstr
3231 + 3];
3232 cartesian_trajectory_planner_B.R_f[cartesian_trajectory_planner_B.loop_ub_n]
3233 +=
3234 cartesian_trajectory_planner_B.Tj[cartesian_trajectory_planner_B.n_c
3235 + 2] *
3236 cartesian_trajectory_planner_B.R_i[cartesian_trajectory_planner_B.b_kstr
3237 + 6];
3238 cartesian_trajectory_planner_B.X[cartesian_trajectory_planner_B.g + 6 *
3239 cartesian_trajectory_planner_B.b_kstr] =
3240 cartesian_trajectory_planner_B.Tj
3241 [(cartesian_trajectory_planner_B.b_kstr << 2) +
3242 cartesian_trajectory_planner_B.g];
3243 cartesian_trajectory_planner_B.X[cartesian_trajectory_planner_B.g + 6 *
3244 (cartesian_trajectory_planner_B.b_kstr + 3)] = 0.0;
3245 }
3246 }
3247
3248 for (cartesian_trajectory_planner_B.b_kstr = 0;
3249 cartesian_trajectory_planner_B.b_kstr < 3;
3250 cartesian_trajectory_planner_B.b_kstr++) {
3251 cartesian_trajectory_planner_B.X[6 *
3252 cartesian_trajectory_planner_B.b_kstr + 3] =
3253 cartesian_trajectory_planner_B.R_f[3 *
3254 cartesian_trajectory_planner_B.b_kstr];
3255 cartesian_trajectory_planner_B.g = cartesian_trajectory_planner_B.b_kstr
3256 << 2;
3257 cartesian_trajectory_planner_B.loop_ub_n = 6 *
3258 (cartesian_trajectory_planner_B.b_kstr + 3);
3259 cartesian_trajectory_planner_B.X[cartesian_trajectory_planner_B.loop_ub_n
3260 + 3] =
3261 cartesian_trajectory_planner_B.Tj[cartesian_trajectory_planner_B.g];
3262 cartesian_trajectory_planner_B.X[6 *
3263 cartesian_trajectory_planner_B.b_kstr + 4] =
3264 cartesian_trajectory_planner_B.R_f[3 *
3265 cartesian_trajectory_planner_B.b_kstr + 1];
3266 cartesian_trajectory_planner_B.X[cartesian_trajectory_planner_B.loop_ub_n
3267 + 4] =
3268 cartesian_trajectory_planner_B.Tj[cartesian_trajectory_planner_B.g + 1];
3269 cartesian_trajectory_planner_B.X[6 *
3270 cartesian_trajectory_planner_B.b_kstr + 5] =
3271 cartesian_trajectory_planner_B.R_f[3 *
3272 cartesian_trajectory_planner_B.b_kstr + 2];
3273 cartesian_trajectory_planner_B.X[cartesian_trajectory_planner_B.loop_ub_n
3274 + 5] =
3275 cartesian_trajectory_planner_B.Tj[cartesian_trajectory_planner_B.g + 2];
3276 }
3277
3278 cartesian_trajectory_planner_B.b_kstr = b->size[0] * b->size[1];
3279 b->size[0] = 6;
3280 b->size[1] = joint->MotionSubspace->size[1];
3281 cartes_emxEnsureCapacity_real_T(b, cartesian_trajectory_planner_B.b_kstr);
3282 cartesian_trajectory_planner_B.loop_ub_n = joint->MotionSubspace->size[0] *
3283 joint->MotionSubspace->size[1] - 1;
3284 for (cartesian_trajectory_planner_B.b_kstr = 0;
3285 cartesian_trajectory_planner_B.b_kstr <=
3286 cartesian_trajectory_planner_B.loop_ub_n;
3287 cartesian_trajectory_planner_B.b_kstr++) {
3288 b->data[cartesian_trajectory_planner_B.b_kstr] = joint->
3289 MotionSubspace->data[cartesian_trajectory_planner_B.b_kstr];
3290 }
3291
3292 cartesian_trajectory_planner_B.n_c = b->size[1] - 1;
3293 cartesian_trajectory_planner_B.b_kstr = y->size[0] * y->size[1];
3294 y->size[0] = 6;
3295 y->size[1] = b->size[1];
3296 cartes_emxEnsureCapacity_real_T(y, cartesian_trajectory_planner_B.b_kstr);
3297 for (cartesian_trajectory_planner_B.b_kstr = 0;
3298 cartesian_trajectory_planner_B.b_kstr <=
3299 cartesian_trajectory_planner_B.n_c;
3300 cartesian_trajectory_planner_B.b_kstr++) {
3301 cartesian_trajectory_planner_B.coffset_tmp =
3302 cartesian_trajectory_planner_B.b_kstr * 6 - 1;
3303 for (cartesian_trajectory_planner_B.g = 0;
3304 cartesian_trajectory_planner_B.g < 6;
3305 cartesian_trajectory_planner_B.g++) {
3306 cartesian_trajectory_planner_B.bid2 = 0.0;
3307 for (cartesian_trajectory_planner_B.loop_ub_n = 0;
3308 cartesian_trajectory_planner_B.loop_ub_n < 6;
3309 cartesian_trajectory_planner_B.loop_ub_n++) {
3310 cartesian_trajectory_planner_B.bid2 +=
3311 cartesian_trajectory_planner_B.X[cartesian_trajectory_planner_B.loop_ub_n
3312 * 6 + cartesian_trajectory_planner_B.g] * b->data
3313 [(cartesian_trajectory_planner_B.coffset_tmp +
3314 cartesian_trajectory_planner_B.loop_ub_n) + 1];
3315 }
3316
3317 y->data[(cartesian_trajectory_planner_B.coffset_tmp +
3318 cartesian_trajectory_planner_B.g) + 1] =
3319 cartesian_trajectory_planner_B.bid2;
3320 }
3321 }
3322
3323 if (cartesian_trajectory_planner_B.bid1 >
3324 cartesian_trajectory_planner_B.qidx_idx_1) {
3325 cartesian_trajectory_planner_B.n_c = 0;
3326 } else {
3327 cartesian_trajectory_planner_B.n_c = static_cast<int32_T>
3328 (cartesian_trajectory_planner_B.bid1) - 1;
3329 }
3330
3331 cartesian_trajectory_planner_B.loop_ub_n = y->size[1];
3332 for (cartesian_trajectory_planner_B.b_kstr = 0;
3333 cartesian_trajectory_planner_B.b_kstr <
3334 cartesian_trajectory_planner_B.loop_ub_n;
3335 cartesian_trajectory_planner_B.b_kstr++) {
3336 for (cartesian_trajectory_planner_B.g = 0;
3337 cartesian_trajectory_planner_B.g < 6;
3338 cartesian_trajectory_planner_B.g++) {
3339 Jac->data[cartesian_trajectory_planner_B.g + 6 *
3340 (cartesian_trajectory_planner_B.n_c +
3341 cartesian_trajectory_planner_B.b_kstr)] = y->data[6 *
3342 cartesian_trajectory_planner_B.b_kstr +
3343 cartesian_trajectory_planner_B.g] * static_cast<real_T>
3344 (cartesian_trajectory_planner_B.jointSign);
3345 }
3346 }
3347 }
3348
3349 if (cartesian_trajectory_planner_B.nextBodyIsParent) {
3350 for (cartesian_trajectory_planner_B.b_kstr = 0;
3351 cartesian_trajectory_planner_B.b_kstr < 4;
3352 cartesian_trajectory_planner_B.b_kstr++) {
3353 for (cartesian_trajectory_planner_B.g = 0;
3354 cartesian_trajectory_planner_B.g < 4;
3355 cartesian_trajectory_planner_B.g++) {
3356 cartesian_trajectory_planner_B.loop_ub_n =
3357 cartesian_trajectory_planner_B.g << 2;
3358 cartesian_trajectory_planner_B.jointSign =
3359 cartesian_trajectory_planner_B.b_kstr +
3360 cartesian_trajectory_planner_B.loop_ub_n;
3361 cartesian_trajectory_planner_B.T1j[cartesian_trajectory_planner_B.jointSign]
3362 = 0.0;
3363 cartesian_trajectory_planner_B.T1j[cartesian_trajectory_planner_B.jointSign]
3364 +=
3365 cartesian_trajectory_planner_B.T1[cartesian_trajectory_planner_B.loop_ub_n]
3366 * cartesian_trajectory_planner_B.Tc2p[cartesian_trajectory_planner_B.b_kstr];
3367 cartesian_trajectory_planner_B.T1j[cartesian_trajectory_planner_B.jointSign]
3368 +=
3369 cartesian_trajectory_planner_B.T1[cartesian_trajectory_planner_B.loop_ub_n
3370 + 1] *
3371 cartesian_trajectory_planner_B.Tc2p[cartesian_trajectory_planner_B.b_kstr
3372 + 4];
3373 cartesian_trajectory_planner_B.T1j[cartesian_trajectory_planner_B.jointSign]
3374 +=
3375 cartesian_trajectory_planner_B.T1[cartesian_trajectory_planner_B.loop_ub_n
3376 + 2] *
3377 cartesian_trajectory_planner_B.Tc2p[cartesian_trajectory_planner_B.b_kstr
3378 + 8];
3379 cartesian_trajectory_planner_B.T1j[cartesian_trajectory_planner_B.jointSign]
3380 +=
3381 cartesian_trajectory_planner_B.T1[cartesian_trajectory_planner_B.loop_ub_n
3382 + 3] *
3383 cartesian_trajectory_planner_B.Tc2p[cartesian_trajectory_planner_B.b_kstr
3384 + 12];
3385 }
3386 }
3387
3388 memcpy(&cartesian_trajectory_planner_B.T1[0],
3389 &cartesian_trajectory_planner_B.T1j[0], sizeof(real_T) << 4U);
3390 } else {
3391 for (cartesian_trajectory_planner_B.b_kstr = 0;
3392 cartesian_trajectory_planner_B.b_kstr < 3;
3393 cartesian_trajectory_planner_B.b_kstr++) {
3394 cartesian_trajectory_planner_B.R_i[3 *
3395 cartesian_trajectory_planner_B.b_kstr] =
3396 cartesian_trajectory_planner_B.Tc2p[cartesian_trajectory_planner_B.b_kstr];
3397 cartesian_trajectory_planner_B.R_i[3 *
3398 cartesian_trajectory_planner_B.b_kstr + 1] =
3399 cartesian_trajectory_planner_B.Tc2p[cartesian_trajectory_planner_B.b_kstr
3400 + 4];
3401 cartesian_trajectory_planner_B.R_i[3 *
3402 cartesian_trajectory_planner_B.b_kstr + 2] =
3403 cartesian_trajectory_planner_B.Tc2p[cartesian_trajectory_planner_B.b_kstr
3404 + 8];
3405 }
3406
3407 for (cartesian_trajectory_planner_B.b_kstr = 0;
3408 cartesian_trajectory_planner_B.b_kstr < 9;
3409 cartesian_trajectory_planner_B.b_kstr++) {
3410 cartesian_trajectory_planner_B.R_f[cartesian_trajectory_planner_B.b_kstr]
3411 =
3412 -cartesian_trajectory_planner_B.R_i[cartesian_trajectory_planner_B.b_kstr];
3413 }
3414
3415 for (cartesian_trajectory_planner_B.b_kstr = 0;
3416 cartesian_trajectory_planner_B.b_kstr < 3;
3417 cartesian_trajectory_planner_B.b_kstr++) {
3418 cartesian_trajectory_planner_B.loop_ub_n =
3419 cartesian_trajectory_planner_B.b_kstr << 2;
3420 cartesian_trajectory_planner_B.T1j[cartesian_trajectory_planner_B.loop_ub_n]
3421 = cartesian_trajectory_planner_B.R_i[3 *
3422 cartesian_trajectory_planner_B.b_kstr];
3423 cartesian_trajectory_planner_B.T1j[cartesian_trajectory_planner_B.loop_ub_n
3424 + 1] = cartesian_trajectory_planner_B.R_i[3 *
3425 cartesian_trajectory_planner_B.b_kstr + 1];
3426 cartesian_trajectory_planner_B.T1j[cartesian_trajectory_planner_B.loop_ub_n
3427 + 2] = cartesian_trajectory_planner_B.R_i[3 *
3428 cartesian_trajectory_planner_B.b_kstr + 2];
3429 cartesian_trajectory_planner_B.T1j[cartesian_trajectory_planner_B.b_kstr
3430 + 12] =
3431 cartesian_trajectory_planner_B.R_f[cartesian_trajectory_planner_B.b_kstr
3432 + 6] * cartesian_trajectory_planner_B.Tc2p[14] +
3433 (cartesian_trajectory_planner_B.R_f[cartesian_trajectory_planner_B.b_kstr
3434 + 3] * cartesian_trajectory_planner_B.Tc2p[13] +
3435 cartesian_trajectory_planner_B.R_f[cartesian_trajectory_planner_B.b_kstr]
3436 * cartesian_trajectory_planner_B.Tc2p[12]);
3437 }
3438
3439 cartesian_trajectory_planner_B.T1j[3] = 0.0;
3440 cartesian_trajectory_planner_B.T1j[7] = 0.0;
3441 cartesian_trajectory_planner_B.T1j[11] = 0.0;
3442 cartesian_trajectory_planner_B.T1j[15] = 1.0;
3443 for (cartesian_trajectory_planner_B.b_kstr = 0;
3444 cartesian_trajectory_planner_B.b_kstr < 4;
3445 cartesian_trajectory_planner_B.b_kstr++) {
3446 for (cartesian_trajectory_planner_B.g = 0;
3447 cartesian_trajectory_planner_B.g < 4;
3448 cartesian_trajectory_planner_B.g++) {
3449 cartesian_trajectory_planner_B.jointSign =
3450 cartesian_trajectory_planner_B.g << 2;
3451 cartesian_trajectory_planner_B.loop_ub_n =
3452 cartesian_trajectory_planner_B.b_kstr +
3453 cartesian_trajectory_planner_B.jointSign;
3454 cartesian_trajectory_planner_B.Tc2p[cartesian_trajectory_planner_B.loop_ub_n]
3455 = 0.0;
3456 cartesian_trajectory_planner_B.Tc2p[cartesian_trajectory_planner_B.loop_ub_n]
3457 +=
3458 cartesian_trajectory_planner_B.T1[cartesian_trajectory_planner_B.jointSign]
3459 * cartesian_trajectory_planner_B.T1j[cartesian_trajectory_planner_B.b_kstr];
3460 cartesian_trajectory_planner_B.Tc2p[cartesian_trajectory_planner_B.loop_ub_n]
3461 +=
3462 cartesian_trajectory_planner_B.T1[cartesian_trajectory_planner_B.jointSign
3463 + 1] *
3464 cartesian_trajectory_planner_B.T1j[cartesian_trajectory_planner_B.b_kstr
3465 + 4];
3466 cartesian_trajectory_planner_B.Tc2p[cartesian_trajectory_planner_B.loop_ub_n]
3467 +=
3468 cartesian_trajectory_planner_B.T1[cartesian_trajectory_planner_B.jointSign
3469 + 2] *
3470 cartesian_trajectory_planner_B.T1j[cartesian_trajectory_planner_B.b_kstr
3471 + 8];
3472 cartesian_trajectory_planner_B.Tc2p[cartesian_trajectory_planner_B.loop_ub_n]
3473 +=
3474 cartesian_trajectory_planner_B.T1[cartesian_trajectory_planner_B.jointSign
3475 + 3] *
3476 cartesian_trajectory_planner_B.T1j[cartesian_trajectory_planner_B.b_kstr
3477 + 12];
3478 }
3479 }
3480
3481 memcpy(&cartesian_trajectory_planner_B.T1[0],
3482 &cartesian_trajectory_planner_B.Tc2p[0], sizeof(real_T) << 4U);
3483 }
3484 }
3485
3486 cartesian_trajec_emxFree_real_T(&b);
3487 cartesian_trajec_emxFree_char_T(&body2Name);
3488 cartesian_trajec_emxFree_real_T(&kinematicPathIndices);
3489 for (cartesian_trajectory_planner_B.b_kstr = 0;
3490 cartesian_trajectory_planner_B.b_kstr < 3;
3491 cartesian_trajectory_planner_B.b_kstr++) {
3492 cartesian_trajectory_planner_B.b_i_f = cartesian_trajectory_planner_B.b_kstr
3493 << 2;
3494 cartesian_trajectory_planner_B.X[6 * cartesian_trajectory_planner_B.b_kstr] =
3495 cartesian_trajectory_planner_B.T1[cartesian_trajectory_planner_B.b_i_f];
3496 cartesian_trajectory_planner_B.g = 6 *
3497 (cartesian_trajectory_planner_B.b_kstr + 3);
3498 cartesian_trajectory_planner_B.X[cartesian_trajectory_planner_B.g] = 0.0;
3499 cartesian_trajectory_planner_B.X[6 * cartesian_trajectory_planner_B.b_kstr +
3500 3] = 0.0;
3501 cartesian_trajectory_planner_B.X[cartesian_trajectory_planner_B.g + 3] =
3502 cartesian_trajectory_planner_B.T1[cartesian_trajectory_planner_B.b_i_f];
3503 cartesian_trajectory_planner_B.bid1 =
3504 cartesian_trajectory_planner_B.T1[cartesian_trajectory_planner_B.b_i_f + 1];
3505 cartesian_trajectory_planner_B.X[6 * cartesian_trajectory_planner_B.b_kstr +
3506 1] = cartesian_trajectory_planner_B.bid1;
3507 cartesian_trajectory_planner_B.X[cartesian_trajectory_planner_B.g + 1] = 0.0;
3508 cartesian_trajectory_planner_B.X[6 * cartesian_trajectory_planner_B.b_kstr +
3509 4] = 0.0;
3510 cartesian_trajectory_planner_B.X[cartesian_trajectory_planner_B.g + 4] =
3511 cartesian_trajectory_planner_B.bid1;
3512 cartesian_trajectory_planner_B.bid1 =
3513 cartesian_trajectory_planner_B.T1[cartesian_trajectory_planner_B.b_i_f + 2];
3514 cartesian_trajectory_planner_B.X[6 * cartesian_trajectory_planner_B.b_kstr +
3515 2] = cartesian_trajectory_planner_B.bid1;
3516 cartesian_trajectory_planner_B.X[cartesian_trajectory_planner_B.g + 2] = 0.0;
3517 cartesian_trajectory_planner_B.X[6 * cartesian_trajectory_planner_B.b_kstr +
3518 5] = 0.0;
3519 cartesian_trajectory_planner_B.X[cartesian_trajectory_planner_B.g + 5] =
3520 cartesian_trajectory_planner_B.bid1;
3521 }
3522
3523 cartesian_trajectory_planner_B.n_c = Jac->size[1];
3524 cartesian_trajectory_planner_B.b_kstr = y->size[0] * y->size[1];
3525 y->size[0] = 6;
3526 y->size[1] = Jac->size[1];
3527 cartes_emxEnsureCapacity_real_T(y, cartesian_trajectory_planner_B.b_kstr);
3528 cartesian_trajectory_planner_B.loop_ub_n = Jac->size[0] * Jac->size[1] - 1;
3529 for (cartesian_trajectory_planner_B.b_kstr = 0;
3530 cartesian_trajectory_planner_B.b_kstr <=
3531 cartesian_trajectory_planner_B.loop_ub_n;
3532 cartesian_trajectory_planner_B.b_kstr++) {
3533 y->data[cartesian_trajectory_planner_B.b_kstr] = Jac->
3534 data[cartesian_trajectory_planner_B.b_kstr];
3535 }
3536
3537 cartesian_trajectory_planner_B.b_kstr = Jac->size[0] * Jac->size[1];
3538 Jac->size[0] = 6;
3539 Jac->size[1] = cartesian_trajectory_planner_B.n_c;
3540 cartes_emxEnsureCapacity_real_T(Jac, cartesian_trajectory_planner_B.b_kstr);
3541 for (cartesian_trajectory_planner_B.b_kstr = 0;
3542 cartesian_trajectory_planner_B.b_kstr <
3543 cartesian_trajectory_planner_B.n_c; cartesian_trajectory_planner_B.b_kstr
3544 ++) {
3545 cartesian_trajectory_planner_B.coffset_tmp =
3546 cartesian_trajectory_planner_B.b_kstr * 6 - 1;
3547 for (cartesian_trajectory_planner_B.b_i_f = 0;
3548 cartesian_trajectory_planner_B.b_i_f < 6;
3549 cartesian_trajectory_planner_B.b_i_f++) {
3550 cartesian_trajectory_planner_B.bid2 = 0.0;
3551 for (cartesian_trajectory_planner_B.loop_ub_n = 0;
3552 cartesian_trajectory_planner_B.loop_ub_n < 6;
3553 cartesian_trajectory_planner_B.loop_ub_n++) {
3554 cartesian_trajectory_planner_B.bid2 +=
3555 cartesian_trajectory_planner_B.X[cartesian_trajectory_planner_B.loop_ub_n
3556 * 6 + cartesian_trajectory_planner_B.b_i_f] * y->data
3557 [(cartesian_trajectory_planner_B.coffset_tmp +
3558 cartesian_trajectory_planner_B.loop_ub_n) + 1];
3559 }
3560
3561 Jac->data[(cartesian_trajectory_planner_B.coffset_tmp +
3562 cartesian_trajectory_planner_B.b_i_f) + 1] =
3563 cartesian_trajectory_planner_B.bid2;
3564 }
3565 }
3566
3567 cartesian_trajec_emxFree_real_T(&y);
3568 T_size[0] = 4;
3569 T_size[1] = 4;
3570 memcpy(&T_data[0], &cartesian_trajectory_planner_B.T1[0], sizeof(real_T) << 4U);
3571}
3572
3573real_T rt_hypotd_snf(real_T u0, real_T u1)
3574{
3575 real_T y;
3576 real_T a;
3577 a = fabs(u0);
3578 y = fabs(u1);
3579 if (a < y) {
3580 a /= y;
3581 y *= sqrt(a * a + 1.0);
3582 } else if (a > y) {
3583 y /= a;
3584 y = sqrt(y * y + 1.0) * a;
3585 } else {
3586 if (!rtIsNaN(y)) {
3587 y = a * 1.4142135623730951;
3588 }
3589 }
3590
3591 return y;
3592}
3593
3594static creal_T cartesian_trajectory_plann_sqrt(const creal_T x)
3595{
3596 creal_T b_x;
3597 real_T absxr;
3598 real_T absxi;
3599 if (x.im == 0.0) {
3600 if (x.re < 0.0) {
3601 absxr = 0.0;
3602 absxi = sqrt(-x.re);
3603 } else {
3604 absxr = sqrt(x.re);
3605 absxi = 0.0;
3606 }
3607 } else if (x.re == 0.0) {
3608 if (x.im < 0.0) {
3609 absxr = sqrt(-x.im / 2.0);
3610 absxi = -absxr;
3611 } else {
3612 absxr = sqrt(x.im / 2.0);
3613 absxi = absxr;
3614 }
3615 } else if (rtIsNaN(x.re)) {
3616 absxr = x.re;
3617 absxi = x.re;
3618 } else if (rtIsNaN(x.im)) {
3619 absxr = x.im;
3620 absxi = x.im;
3621 } else if (rtIsInf(x.im)) {
3622 absxr = fabs(x.im);
3623 absxi = x.im;
3624 } else if (rtIsInf(x.re)) {
3625 if (x.re < 0.0) {
3626 absxr = 0.0;
3627 absxi = x.im * -x.re;
3628 } else {
3629 absxr = x.re;
3630 absxi = 0.0;
3631 }
3632 } else {
3633 absxr = fabs(x.re);
3634 absxi = fabs(x.im);
3635 if ((absxr > 4.4942328371557893E+307) || (absxi > 4.4942328371557893E+307))
3636 {
3637 absxr *= 0.5;
3638 absxi *= 0.5;
3639 absxi = rt_hypotd_snf(absxr, absxi);
3640 if (absxi > absxr) {
3641 absxr = sqrt(absxr / absxi + 1.0) * sqrt(absxi);
3642 } else {
3643 absxr = sqrt(absxi) * 1.4142135623730951;
3644 }
3645 } else {
3646 absxr = sqrt((rt_hypotd_snf(absxr, absxi) + absxr) * 0.5);
3647 }
3648
3649 if (x.re > 0.0) {
3650 absxi = x.im / absxr * 0.5;
3651 } else {
3652 if (x.im < 0.0) {
3653 absxi = -absxr;
3654 } else {
3655 absxi = absxr;
3656 }
3657
3658 absxr = x.im / absxi * 0.5;
3659 }
3660 }
3661
3662 b_x.re = absxr;
3663 b_x.im = absxi;
3664 return b_x;
3665}
3666
3667real_T rt_atan2d_snf(real_T u0, real_T u1)
3668{
3669 real_T y;
3670 int32_T u0_0;
3671 int32_T u1_0;
3672 if (rtIsNaN(u0) || rtIsNaN(u1)) {
3673 y = (rtNaN);
3674 } else if (rtIsInf(u0) && rtIsInf(u1)) {
3675 if (u0 > 0.0) {
3676 u0_0 = 1;
3677 } else {
3678 u0_0 = -1;
3679 }
3680
3681 if (u1 > 0.0) {
3682 u1_0 = 1;
3683 } else {
3684 u1_0 = -1;
3685 }
3686
3687 y = atan2(static_cast<real_T>(u0_0), static_cast<real_T>(u1_0));
3688 } else if (u1 == 0.0) {
3689 if (u0 > 0.0) {
3690 y = RT_PI / 2.0;
3691 } else if (u0 < 0.0) {
3692 y = -(RT_PI / 2.0);
3693 } else {
3694 y = 0.0;
3695 }
3696 } else {
3697 y = atan2(u0, u1);
3698 }
3699
3700 return y;
3701}
3702
3703static real_T cartesian_trajectory_plan_xnrm2(int32_T n, const real_T x[9],
3704 int32_T ix0)
3705{
3706 real_T y;
3707 real_T scale;
3708 int32_T kend;
3709 real_T absxk;
3710 real_T t;
3711 int32_T k;
3712 y = 0.0;
3713 scale = 3.3121686421112381E-170;
3714 kend = ix0 + n;
3715 for (k = ix0; k < kend; k++) {
3716 absxk = fabs(x[k - 1]);
3717 if (absxk > scale) {
3718 t = scale / absxk;
3719 y = y * t * t + 1.0;
3720 scale = absxk;
3721 } else {
3722 t = absxk / scale;
3723 y += t * t;
3724 }
3725 }
3726
3727 return scale * sqrt(y);
3728}
3729
3730static real_T cartesian_trajectory_plan_xdotc(int32_T n, const real_T x[9],
3731 int32_T ix0, const real_T y[9], int32_T iy0)
3732{
3733 real_T d;
3734 int32_T ix;
3735 int32_T iy;
3736 int32_T k;
3737 d = 0.0;
3738 ix = ix0 - 1;
3739 iy = iy0 - 1;
3740 for (k = 0; k < n; k++) {
3741 d += x[ix] * y[iy];
3742 ix++;
3743 iy++;
3744 }
3745
3746 return d;
3747}
3748
3749static void cartesian_trajectory_plan_xaxpy(int32_T n, real_T a, int32_T ix0,
3750 const real_T y[9], int32_T iy0, real_T b_y[9])
3751{
3752 int32_T ix;
3753 int32_T iy;
3754 int32_T k;
3755 memcpy(&b_y[0], &y[0], 9U * sizeof(real_T));
3756 if (!(a == 0.0)) {
3757 ix = ix0;
3758 iy = iy0 - 1;
3759 for (k = 0; k < n; k++) {
3760 b_y[iy] += b_y[ix - 1] * a;
3761 ix++;
3762 iy++;
3763 }
3764 }
3765}
3766
3767static real_T cartesian_trajectory_pl_xnrm2_a(const real_T x[3], int32_T ix0)
3768{
3769 real_T y;
3770 real_T scale;
3771 real_T absxk;
3772 real_T t;
3773 int32_T k;
3774 y = 0.0;
3775 scale = 3.3121686421112381E-170;
3776 for (k = ix0; k <= ix0 + 1; k++) {
3777 absxk = fabs(x[k - 1]);
3778 if (absxk > scale) {
3779 t = scale / absxk;
3780 y = y * t * t + 1.0;
3781 scale = absxk;
3782 } else {
3783 t = absxk / scale;
3784 y += t * t;
3785 }
3786 }
3787
3788 return scale * sqrt(y);
3789}
3790
3791static void cartesian_trajectory__xaxpy_ast(int32_T n, real_T a, const real_T x
3792 [9], int32_T ix0, real_T y[3], int32_T iy0)
3793{
3794 int32_T ix;
3795 int32_T iy;
3796 int32_T k;
3797 if (!(a == 0.0)) {
3798 ix = ix0;
3799 iy = iy0 - 1;
3800 for (k = 0; k < n; k++) {
3801 y[iy] += x[ix - 1] * a;
3802 ix++;
3803 iy++;
3804 }
3805 }
3806}
3807
3808static void cartesian_trajectory_p_xaxpy_as(int32_T n, real_T a, const real_T x
3809 [3], int32_T ix0, const real_T y[9], int32_T iy0, real_T b_y[9])
3810{
3811 int32_T ix;
3812 int32_T iy;
3813 int32_T k;
3814 memcpy(&b_y[0], &y[0], 9U * sizeof(real_T));
3815 if (!(a == 0.0)) {
3816 ix = ix0;
3817 iy = iy0 - 1;
3818 for (k = 0; k < n; k++) {
3819 b_y[iy] += x[ix - 1] * a;
3820 ix++;
3821 iy++;
3822 }
3823 }
3824}
3825
3826static void cartesian_trajectory_plan_xswap(const real_T x[9], int32_T ix0,
3827 int32_T iy0, real_T b_x[9])
3828{
3829 int32_T ix;
3830 int32_T iy;
3831 real_T temp;
3832 memcpy(&b_x[0], &x[0], 9U * sizeof(real_T));
3833 ix = ix0 - 1;
3834 iy = iy0 - 1;
3835 temp = b_x[ix];
3836 b_x[ix] = b_x[iy];
3837 b_x[iy] = temp;
3838 ix++;
3839 iy++;
3840 temp = b_x[ix];
3841 b_x[ix] = b_x[iy];
3842 b_x[iy] = temp;
3843 ix++;
3844 iy++;
3845 temp = b_x[ix];
3846 b_x[ix] = b_x[iy];
3847 b_x[iy] = temp;
3848}
3849
3850static void cartesian_trajectory_plan_xrotg(real_T a, real_T b, real_T *b_a,
3851 real_T *b_b, real_T *c, real_T *s)
3852{
3853 cartesian_trajectory_planner_B.roe = b;
3854 cartesian_trajectory_planner_B.absa = fabs(a);
3855 cartesian_trajectory_planner_B.absb = fabs(b);
3856 if (cartesian_trajectory_planner_B.absa > cartesian_trajectory_planner_B.absb)
3857 {
3858 cartesian_trajectory_planner_B.roe = a;
3859 }
3860
3861 cartesian_trajectory_planner_B.scale = cartesian_trajectory_planner_B.absa +
3862 cartesian_trajectory_planner_B.absb;
3863 if (cartesian_trajectory_planner_B.scale == 0.0) {
3864 *s = 0.0;
3865 *c = 1.0;
3866 *b_a = 0.0;
3867 *b_b = 0.0;
3868 } else {
3869 cartesian_trajectory_planner_B.ads = cartesian_trajectory_planner_B.absa /
3870 cartesian_trajectory_planner_B.scale;
3871 cartesian_trajectory_planner_B.bds = cartesian_trajectory_planner_B.absb /
3872 cartesian_trajectory_planner_B.scale;
3873 *b_a = sqrt(cartesian_trajectory_planner_B.ads *
3874 cartesian_trajectory_planner_B.ads +
3875 cartesian_trajectory_planner_B.bds *
3876 cartesian_trajectory_planner_B.bds) *
3877 cartesian_trajectory_planner_B.scale;
3878 if (cartesian_trajectory_planner_B.roe < 0.0) {
3879 *b_a = -*b_a;
3880 }
3881
3882 *c = a / *b_a;
3883 *s = b / *b_a;
3884 if (cartesian_trajectory_planner_B.absa >
3885 cartesian_trajectory_planner_B.absb) {
3886 *b_b = *s;
3887 } else if (*c != 0.0) {
3888 *b_b = 1.0 / *c;
3889 } else {
3890 *b_b = 1.0;
3891 }
3892 }
3893}
3894
3895static void cartesian_trajectory_plann_xrot(const real_T x[9], int32_T ix0,
3896 int32_T iy0, real_T c, real_T s, real_T b_x[9])
3897{
3898 int32_T ix;
3899 int32_T iy;
3900 real_T temp;
3901 memcpy(&b_x[0], &x[0], 9U * sizeof(real_T));
3902 ix = ix0 - 1;
3903 iy = iy0 - 1;
3904 temp = c * b_x[ix] + s * b_x[iy];
3905 b_x[iy] = c * b_x[iy] - s * b_x[ix];
3906 b_x[ix] = temp;
3907 iy++;
3908 ix++;
3909 temp = c * b_x[ix] + s * b_x[iy];
3910 b_x[iy] = c * b_x[iy] - s * b_x[ix];
3911 b_x[ix] = temp;
3912 iy++;
3913 ix++;
3914 temp = c * b_x[ix] + s * b_x[iy];
3915 b_x[iy] = c * b_x[iy] - s * b_x[ix];
3916 b_x[ix] = temp;
3917}
3918
3919static void cartesian_trajectory_planne_svd(const real_T A[9], real_T U[9],
3920 real_T s[3], real_T V[9])
3921{
3922 int32_T qq;
3923 boolean_T apply_transform;
3924 int32_T qjj;
3925 int32_T m;
3926 int32_T kase;
3927 int32_T c_q;
3928 int32_T d_k;
3929 boolean_T exitg1;
3930 cartesian_trajectory_planner_B.e_j[0] = 0.0;
3931 cartesian_trajectory_planner_B.work[0] = 0.0;
3932 cartesian_trajectory_planner_B.e_j[1] = 0.0;
3933 cartesian_trajectory_planner_B.work[1] = 0.0;
3934 cartesian_trajectory_planner_B.e_j[2] = 0.0;
3935 cartesian_trajectory_planner_B.work[2] = 0.0;
3936 for (m = 0; m < 9; m++) {
3937 cartesian_trajectory_planner_B.A[m] = A[m];
3938 U[m] = 0.0;
3939 V[m] = 0.0;
3940 }
3941
3942 apply_transform = false;
3943 cartesian_trajectory_planner_B.nrm = cartesian_trajectory_plan_xnrm2(3,
3944 cartesian_trajectory_planner_B.A, 1);
3945 if (cartesian_trajectory_planner_B.nrm > 0.0) {
3946 apply_transform = true;
3947 if (cartesian_trajectory_planner_B.A[0] < 0.0) {
3948 cartesian_trajectory_planner_B.s[0] = -cartesian_trajectory_planner_B.nrm;
3949 } else {
3950 cartesian_trajectory_planner_B.s[0] = cartesian_trajectory_planner_B.nrm;
3951 }
3952
3953 if (fabs(cartesian_trajectory_planner_B.s[0]) >= 1.0020841800044864E-292) {
3954 cartesian_trajectory_planner_B.nrm = 1.0 /
3955 cartesian_trajectory_planner_B.s[0];
3956 for (qq = 1; qq < 4; qq++) {
3957 cartesian_trajectory_planner_B.A[qq - 1] *=
3958 cartesian_trajectory_planner_B.nrm;
3959 }
3960 } else {
3961 for (qq = 1; qq < 4; qq++) {
3962 cartesian_trajectory_planner_B.A[qq - 1] /=
3963 cartesian_trajectory_planner_B.s[0];
3964 }
3965 }
3966
3967 cartesian_trajectory_planner_B.A[0]++;
3968 cartesian_trajectory_planner_B.s[0] = -cartesian_trajectory_planner_B.s[0];
3969 } else {
3970 cartesian_trajectory_planner_B.s[0] = 0.0;
3971 }
3972
3973 for (m = 2; m < 4; m++) {
3974 qjj = (m - 1) * 3 + 1;
3975 if (apply_transform) {
3976 memcpy(&cartesian_trajectory_planner_B.A_e[0],
3977 &cartesian_trajectory_planner_B.A[0], 9U * sizeof(real_T));
3978 cartesian_trajectory_plan_xaxpy(3, -(cartesian_trajectory_plan_xdotc(3,
3979 cartesian_trajectory_planner_B.A, 1, cartesian_trajectory_planner_B.A,
3980 qjj) / cartesian_trajectory_planner_B.A[0]), 1,
3981 cartesian_trajectory_planner_B.A_e, qjj,
3982 cartesian_trajectory_planner_B.A);
3983 }
3984
3985 cartesian_trajectory_planner_B.e_j[m - 1] =
3986 cartesian_trajectory_planner_B.A[qjj - 1];
3987 }
3988
3989 for (m = 1; m < 4; m++) {
3990 U[m - 1] = cartesian_trajectory_planner_B.A[m - 1];
3991 }
3992
3993 cartesian_trajectory_planner_B.nrm = cartesian_trajectory_pl_xnrm2_a
3994 (cartesian_trajectory_planner_B.e_j, 2);
3995 if (cartesian_trajectory_planner_B.nrm == 0.0) {
3996 cartesian_trajectory_planner_B.e_j[0] = 0.0;
3997 } else {
3998 if (cartesian_trajectory_planner_B.e_j[1] < 0.0) {
3999 cartesian_trajectory_planner_B.rt = -cartesian_trajectory_planner_B.nrm;
4000 cartesian_trajectory_planner_B.e_j[0] =
4001 -cartesian_trajectory_planner_B.nrm;
4002 } else {
4003 cartesian_trajectory_planner_B.rt = cartesian_trajectory_planner_B.nrm;
4004 cartesian_trajectory_planner_B.e_j[0] = cartesian_trajectory_planner_B.nrm;
4005 }
4006
4007 if (fabs(cartesian_trajectory_planner_B.rt) >= 1.0020841800044864E-292) {
4008 cartesian_trajectory_planner_B.nrm = 1.0 /
4009 cartesian_trajectory_planner_B.rt;
4010 for (qq = 2; qq < 4; qq++) {
4011 cartesian_trajectory_planner_B.e_j[qq - 1] *=
4012 cartesian_trajectory_planner_B.nrm;
4013 }
4014 } else {
4015 for (qq = 2; qq < 4; qq++) {
4016 cartesian_trajectory_planner_B.e_j[qq - 1] /=
4017 cartesian_trajectory_planner_B.rt;
4018 }
4019 }
4020
4021 cartesian_trajectory_planner_B.e_j[1]++;
4022 cartesian_trajectory_planner_B.e_j[0] = -cartesian_trajectory_planner_B.e_j
4023 [0];
4024 for (m = 2; m < 4; m++) {
4025 cartesian_trajectory_planner_B.work[m - 1] = 0.0;
4026 }
4027
4028 for (m = 2; m < 4; m++) {
4029 cartesian_trajectory__xaxpy_ast(2, cartesian_trajectory_planner_B.e_j[m -
4030 1], cartesian_trajectory_planner_B.A, 3 * (m - 1) + 2,
4031 cartesian_trajectory_planner_B.work, 2);
4032 }
4033
4034 for (m = 2; m < 4; m++) {
4035 memcpy(&cartesian_trajectory_planner_B.A_e[0],
4036 &cartesian_trajectory_planner_B.A[0], 9U * sizeof(real_T));
4037 cartesian_trajectory_p_xaxpy_as(2, -cartesian_trajectory_planner_B.e_j[m -
4038 1] / cartesian_trajectory_planner_B.e_j[1],
4039 cartesian_trajectory_planner_B.work, 2,
4040 cartesian_trajectory_planner_B.A_e, (m - 1) * 3 + 2,
4041 cartesian_trajectory_planner_B.A);
4042 }
4043 }
4044
4045 for (m = 2; m < 4; m++) {
4046 V[m - 1] = cartesian_trajectory_planner_B.e_j[m - 1];
4047 }
4048
4049 apply_transform = false;
4050 cartesian_trajectory_planner_B.nrm = cartesian_trajectory_plan_xnrm2(2,
4051 cartesian_trajectory_planner_B.A, 5);
4052 if (cartesian_trajectory_planner_B.nrm > 0.0) {
4053 apply_transform = true;
4054 if (cartesian_trajectory_planner_B.A[4] < 0.0) {
4055 cartesian_trajectory_planner_B.s[1] = -cartesian_trajectory_planner_B.nrm;
4056 } else {
4057 cartesian_trajectory_planner_B.s[1] = cartesian_trajectory_planner_B.nrm;
4058 }
4059
4060 if (fabs(cartesian_trajectory_planner_B.s[1]) >= 1.0020841800044864E-292) {
4061 cartesian_trajectory_planner_B.nrm = 1.0 /
4062 cartesian_trajectory_planner_B.s[1];
4063 for (qq = 5; qq < 7; qq++) {
4064 cartesian_trajectory_planner_B.A[qq - 1] *=
4065 cartesian_trajectory_planner_B.nrm;
4066 }
4067 } else {
4068 for (qq = 5; qq < 7; qq++) {
4069 cartesian_trajectory_planner_B.A[qq - 1] /=
4070 cartesian_trajectory_planner_B.s[1];
4071 }
4072 }
4073
4074 cartesian_trajectory_planner_B.A[4]++;
4075 cartesian_trajectory_planner_B.s[1] = -cartesian_trajectory_planner_B.s[1];
4076 } else {
4077 cartesian_trajectory_planner_B.s[1] = 0.0;
4078 }
4079
4080 if (apply_transform) {
4081 for (m = 3; m < 4; m++) {
4082 memcpy(&cartesian_trajectory_planner_B.A_e[0],
4083 &cartesian_trajectory_planner_B.A[0], 9U * sizeof(real_T));
4084 cartesian_trajectory_plan_xaxpy(2, -(cartesian_trajectory_plan_xdotc(2,
4085 cartesian_trajectory_planner_B.A, 5, cartesian_trajectory_planner_B.A, 8)
4086 / cartesian_trajectory_planner_B.A[4]), 5,
4087 cartesian_trajectory_planner_B.A_e, 8, cartesian_trajectory_planner_B.A);
4088 }
4089 }
4090
4091 for (m = 2; m < 4; m++) {
4092 U[m + 2] = cartesian_trajectory_planner_B.A[m + 2];
4093 }
4094
4095 m = 2;
4096 cartesian_trajectory_planner_B.s[2] = cartesian_trajectory_planner_B.A[8];
4097 cartesian_trajectory_planner_B.e_j[1] = cartesian_trajectory_planner_B.A[7];
4098 cartesian_trajectory_planner_B.e_j[2] = 0.0;
4099 U[6] = 0.0;
4100 U[7] = 0.0;
4101 U[8] = 1.0;
4102 for (c_q = 1; c_q >= 0; c_q--) {
4103 qq = 3 * c_q + c_q;
4104 if (cartesian_trajectory_planner_B.s[c_q] != 0.0) {
4105 for (kase = c_q + 2; kase < 4; kase++) {
4106 qjj = ((kase - 1) * 3 + c_q) + 1;
4107 memcpy(&cartesian_trajectory_planner_B.A[0], &U[0], 9U * sizeof(real_T));
4108 cartesian_trajectory_plan_xaxpy(3 - c_q,
4109 -(cartesian_trajectory_plan_xdotc(3 - c_q, U, qq + 1, U, qjj) / U[qq]),
4110 qq + 1, cartesian_trajectory_planner_B.A, qjj, U);
4111 }
4112
4113 for (qjj = c_q + 1; qjj < 4; qjj++) {
4114 kase = (3 * c_q + qjj) - 1;
4115 U[kase] = -U[kase];
4116 }
4117
4118 U[qq]++;
4119 if (0 <= c_q - 1) {
4120 U[3 * c_q] = 0.0;
4121 }
4122 } else {
4123 U[3 * c_q] = 0.0;
4124 U[3 * c_q + 1] = 0.0;
4125 U[3 * c_q + 2] = 0.0;
4126 U[qq] = 1.0;
4127 }
4128 }
4129
4130 for (c_q = 2; c_q >= 0; c_q--) {
4131 if ((c_q + 1 <= 1) && (cartesian_trajectory_planner_B.e_j[0] != 0.0)) {
4132 memcpy(&cartesian_trajectory_planner_B.A[0], &V[0], 9U * sizeof(real_T));
4133 cartesian_trajectory_plan_xaxpy(2, -(cartesian_trajectory_plan_xdotc(2, V,
4134 2, V, 5) / V[1]), 2, cartesian_trajectory_planner_B.A, 5, V);
4135 memcpy(&cartesian_trajectory_planner_B.A[0], &V[0], 9U * sizeof(real_T));
4136 cartesian_trajectory_plan_xaxpy(2, -(cartesian_trajectory_plan_xdotc(2, V,
4137 2, V, 8) / V[1]), 2, cartesian_trajectory_planner_B.A, 8, V);
4138 }
4139
4140 V[3 * c_q] = 0.0;
4141 V[3 * c_q + 1] = 0.0;
4142 V[3 * c_q + 2] = 0.0;
4143 V[c_q + 3 * c_q] = 1.0;
4144 }
4145
4146 for (c_q = 0; c_q < 3; c_q++) {
4147 cartesian_trajectory_planner_B.ztest =
4148 cartesian_trajectory_planner_B.e_j[c_q];
4149 if (cartesian_trajectory_planner_B.s[c_q] != 0.0) {
4150 cartesian_trajectory_planner_B.rt = fabs
4151 (cartesian_trajectory_planner_B.s[c_q]);
4152 cartesian_trajectory_planner_B.nrm = cartesian_trajectory_planner_B.s[c_q]
4153 / cartesian_trajectory_planner_B.rt;
4154 cartesian_trajectory_planner_B.s[c_q] = cartesian_trajectory_planner_B.rt;
4155 if (c_q + 1 < 3) {
4156 cartesian_trajectory_planner_B.ztest =
4157 cartesian_trajectory_planner_B.e_j[c_q] /
4158 cartesian_trajectory_planner_B.nrm;
4159 }
4160
4161 qjj = 3 * c_q;
4162 for (qq = qjj + 1; qq <= qjj + 3; qq++) {
4163 U[qq - 1] *= cartesian_trajectory_planner_B.nrm;
4164 }
4165 }
4166
4167 if ((c_q + 1 < 3) && (cartesian_trajectory_planner_B.ztest != 0.0)) {
4168 cartesian_trajectory_planner_B.rt = fabs
4169 (cartesian_trajectory_planner_B.ztest);
4170 cartesian_trajectory_planner_B.nrm = cartesian_trajectory_planner_B.rt /
4171 cartesian_trajectory_planner_B.ztest;
4172 cartesian_trajectory_planner_B.ztest = cartesian_trajectory_planner_B.rt;
4173 cartesian_trajectory_planner_B.s[c_q + 1] *=
4174 cartesian_trajectory_planner_B.nrm;
4175 qjj = (c_q + 1) * 3;
4176 for (qq = qjj + 1; qq <= qjj + 3; qq++) {
4177 V[qq - 1] *= cartesian_trajectory_planner_B.nrm;
4178 }
4179 }
4180
4181 cartesian_trajectory_planner_B.e_j[c_q] =
4182 cartesian_trajectory_planner_B.ztest;
4183 }
4184
4185 qq = 0;
4186 cartesian_trajectory_planner_B.nrm = 0.0;
4187 cartesian_trajectory_planner_B.ztest = fabs(cartesian_trajectory_planner_B.s[0]);
4188 cartesian_trajectory_planner_B.rt = fabs(cartesian_trajectory_planner_B.e_j[0]);
4189 if ((cartesian_trajectory_planner_B.ztest > cartesian_trajectory_planner_B.rt)
4190 || rtIsNaN(cartesian_trajectory_planner_B.rt)) {
4191 cartesian_trajectory_planner_B.rt = cartesian_trajectory_planner_B.ztest;
4192 }
4193
4194 if (!rtIsNaN(cartesian_trajectory_planner_B.rt)) {
4195 cartesian_trajectory_planner_B.nrm = cartesian_trajectory_planner_B.rt;
4196 }
4197
4198 cartesian_trajectory_planner_B.ztest = fabs(cartesian_trajectory_planner_B.s[1]);
4199 cartesian_trajectory_planner_B.rt = fabs(cartesian_trajectory_planner_B.e_j[1]);
4200 if ((cartesian_trajectory_planner_B.ztest > cartesian_trajectory_planner_B.rt)
4201 || rtIsNaN(cartesian_trajectory_planner_B.rt)) {
4202 cartesian_trajectory_planner_B.rt = cartesian_trajectory_planner_B.ztest;
4203 }
4204
4205 if ((!(cartesian_trajectory_planner_B.nrm > cartesian_trajectory_planner_B.rt))
4206 && (!rtIsNaN(cartesian_trajectory_planner_B.rt))) {
4207 cartesian_trajectory_planner_B.nrm = cartesian_trajectory_planner_B.rt;
4208 }
4209
4210 cartesian_trajectory_planner_B.ztest = fabs(cartesian_trajectory_planner_B.s[2]);
4211 cartesian_trajectory_planner_B.rt = fabs(cartesian_trajectory_planner_B.e_j[2]);
4212 if ((cartesian_trajectory_planner_B.ztest > cartesian_trajectory_planner_B.rt)
4213 || rtIsNaN(cartesian_trajectory_planner_B.rt)) {
4214 cartesian_trajectory_planner_B.rt = cartesian_trajectory_planner_B.ztest;
4215 }
4216
4217 if ((!(cartesian_trajectory_planner_B.nrm > cartesian_trajectory_planner_B.rt))
4218 && (!rtIsNaN(cartesian_trajectory_planner_B.rt))) {
4219 cartesian_trajectory_planner_B.nrm = cartesian_trajectory_planner_B.rt;
4220 }
4221
4222 while ((m + 1 > 0) && (!(qq >= 75))) {
4223 c_q = m;
4224 qjj = m;
4225 exitg1 = false;
4226 while ((!exitg1) && (qjj > -1)) {
4227 c_q = qjj;
4228 if (qjj == 0) {
4229 exitg1 = true;
4230 } else {
4231 cartesian_trajectory_planner_B.rt = fabs
4232 (cartesian_trajectory_planner_B.e_j[qjj - 1]);
4233 if ((cartesian_trajectory_planner_B.rt <= (fabs
4234 (cartesian_trajectory_planner_B.s[qjj - 1]) + fabs
4235 (cartesian_trajectory_planner_B.s[qjj])) * 2.2204460492503131E-16)
4236 || (cartesian_trajectory_planner_B.rt <= 1.0020841800044864E-292) ||
4237 ((qq > 20) && (cartesian_trajectory_planner_B.rt <=
4238 2.2204460492503131E-16 *
4239 cartesian_trajectory_planner_B.nrm))) {
4240 cartesian_trajectory_planner_B.e_j[qjj - 1] = 0.0;
4241 exitg1 = true;
4242 } else {
4243 qjj--;
4244 }
4245 }
4246 }
4247
4248 if (c_q == m) {
4249 kase = 4;
4250 } else {
4251 qjj = m + 1;
4252 kase = m + 1;
4253 exitg1 = false;
4254 while ((!exitg1) && (kase >= c_q)) {
4255 qjj = kase;
4256 if (kase == c_q) {
4257 exitg1 = true;
4258 } else {
4259 cartesian_trajectory_planner_B.rt = 0.0;
4260 if (kase < m + 1) {
4261 cartesian_trajectory_planner_B.rt = fabs
4262 (cartesian_trajectory_planner_B.e_j[kase - 1]);
4263 }
4264
4265 if (kase > c_q + 1) {
4266 cartesian_trajectory_planner_B.rt += fabs
4267 (cartesian_trajectory_planner_B.e_j[kase - 2]);
4268 }
4269
4270 cartesian_trajectory_planner_B.ztest = fabs
4271 (cartesian_trajectory_planner_B.s[kase - 1]);
4272 if ((cartesian_trajectory_planner_B.ztest <= 2.2204460492503131E-16 *
4273 cartesian_trajectory_planner_B.rt) ||
4274 (cartesian_trajectory_planner_B.ztest <= 1.0020841800044864E-292))
4275 {
4276 cartesian_trajectory_planner_B.s[kase - 1] = 0.0;
4277 exitg1 = true;
4278 } else {
4279 kase--;
4280 }
4281 }
4282 }
4283
4284 if (qjj == c_q) {
4285 kase = 3;
4286 } else if (m + 1 == qjj) {
4287 kase = 1;
4288 } else {
4289 kase = 2;
4290 c_q = qjj;
4291 }
4292 }
4293
4294 switch (kase) {
4295 case 1:
4296 cartesian_trajectory_planner_B.rt = cartesian_trajectory_planner_B.e_j[m -
4297 1];
4298 cartesian_trajectory_planner_B.e_j[m - 1] = 0.0;
4299 for (qjj = m; qjj >= c_q + 1; qjj--) {
4300 cartesian_trajectory_planner_B.ztest =
4301 cartesian_trajectory_planner_B.e_j[0];
4302 cartesian_trajectory_plan_xrotg(cartesian_trajectory_planner_B.s[qjj - 1],
4303 cartesian_trajectory_planner_B.rt,
4304 &cartesian_trajectory_planner_B.s[qjj - 1],
4305 &cartesian_trajectory_planner_B.rt,
4306 &cartesian_trajectory_planner_B.sqds,
4307 &cartesian_trajectory_planner_B.b_ob);
4308 if (qjj > c_q + 1) {
4309 cartesian_trajectory_planner_B.rt =
4310 -cartesian_trajectory_planner_B.b_ob *
4311 cartesian_trajectory_planner_B.e_j[0];
4312 cartesian_trajectory_planner_B.ztest =
4313 cartesian_trajectory_planner_B.e_j[0] *
4314 cartesian_trajectory_planner_B.sqds;
4315 }
4316
4317 memcpy(&cartesian_trajectory_planner_B.A[0], &V[0], 9U * sizeof(real_T));
4318 cartesian_trajectory_plann_xrot(cartesian_trajectory_planner_B.A, (qjj -
4319 1) * 3 + 1, 3 * m + 1, cartesian_trajectory_planner_B.sqds,
4320 cartesian_trajectory_planner_B.b_ob, V);
4321 cartesian_trajectory_planner_B.e_j[0] =
4322 cartesian_trajectory_planner_B.ztest;
4323 }
4324 break;
4325
4326 case 2:
4327 cartesian_trajectory_planner_B.rt = cartesian_trajectory_planner_B.e_j[c_q
4328 - 1];
4329 cartesian_trajectory_planner_B.e_j[c_q - 1] = 0.0;
4330 for (qjj = c_q + 1; qjj <= m + 1; qjj++) {
4331 cartesian_trajectory_plan_xrotg(cartesian_trajectory_planner_B.s[qjj - 1],
4332 cartesian_trajectory_planner_B.rt,
4333 &cartesian_trajectory_planner_B.s[qjj - 1],
4334 &cartesian_trajectory_planner_B.ztest,
4335 &cartesian_trajectory_planner_B.sqds,
4336 &cartesian_trajectory_planner_B.b_ob);
4337 cartesian_trajectory_planner_B.ztest =
4338 cartesian_trajectory_planner_B.e_j[qjj - 1];
4339 cartesian_trajectory_planner_B.rt = cartesian_trajectory_planner_B.ztest
4340 * -cartesian_trajectory_planner_B.b_ob;
4341 cartesian_trajectory_planner_B.e_j[qjj - 1] =
4342 cartesian_trajectory_planner_B.ztest *
4343 cartesian_trajectory_planner_B.sqds;
4344 memcpy(&cartesian_trajectory_planner_B.A[0], &U[0], 9U * sizeof(real_T));
4345 cartesian_trajectory_plann_xrot(cartesian_trajectory_planner_B.A, (qjj -
4346 1) * 3 + 1, (c_q - 1) * 3 + 1, cartesian_trajectory_planner_B.sqds,
4347 cartesian_trajectory_planner_B.b_ob, U);
4348 }
4349 break;
4350
4351 case 3:
4352 cartesian_trajectory_planner_B.ztest = fabs
4353 (cartesian_trajectory_planner_B.s[m]);
4354 cartesian_trajectory_planner_B.sqds = cartesian_trajectory_planner_B.s[m -
4355 1];
4356 cartesian_trajectory_planner_B.rt = fabs
4357 (cartesian_trajectory_planner_B.sqds);
4358 if ((cartesian_trajectory_planner_B.ztest >
4359 cartesian_trajectory_planner_B.rt) || rtIsNaN
4360 (cartesian_trajectory_planner_B.rt)) {
4361 cartesian_trajectory_planner_B.rt = cartesian_trajectory_planner_B.ztest;
4362 }
4363
4364 cartesian_trajectory_planner_B.b_ob = cartesian_trajectory_planner_B.e_j[m
4365 - 1];
4366 cartesian_trajectory_planner_B.ztest = fabs
4367 (cartesian_trajectory_planner_B.b_ob);
4368 if ((cartesian_trajectory_planner_B.rt >
4369 cartesian_trajectory_planner_B.ztest) || rtIsNaN
4370 (cartesian_trajectory_planner_B.ztest)) {
4371 cartesian_trajectory_planner_B.ztest = cartesian_trajectory_planner_B.rt;
4372 }
4373
4374 cartesian_trajectory_planner_B.rt = fabs
4375 (cartesian_trajectory_planner_B.s[c_q]);
4376 if ((cartesian_trajectory_planner_B.ztest >
4377 cartesian_trajectory_planner_B.rt) || rtIsNaN
4378 (cartesian_trajectory_planner_B.rt)) {
4379 cartesian_trajectory_planner_B.rt = cartesian_trajectory_planner_B.ztest;
4380 }
4381
4382 cartesian_trajectory_planner_B.ztest = fabs
4383 (cartesian_trajectory_planner_B.e_j[c_q]);
4384 if ((cartesian_trajectory_planner_B.rt >
4385 cartesian_trajectory_planner_B.ztest) || rtIsNaN
4386 (cartesian_trajectory_planner_B.ztest)) {
4387 cartesian_trajectory_planner_B.ztest = cartesian_trajectory_planner_B.rt;
4388 }
4389
4390 cartesian_trajectory_planner_B.rt = cartesian_trajectory_planner_B.s[m] /
4391 cartesian_trajectory_planner_B.ztest;
4392 cartesian_trajectory_planner_B.smm1 = cartesian_trajectory_planner_B.sqds /
4393 cartesian_trajectory_planner_B.ztest;
4394 cartesian_trajectory_planner_B.emm1 = cartesian_trajectory_planner_B.b_ob /
4395 cartesian_trajectory_planner_B.ztest;
4396 cartesian_trajectory_planner_B.sqds = cartesian_trajectory_planner_B.s[c_q]
4397 / cartesian_trajectory_planner_B.ztest;
4398 cartesian_trajectory_planner_B.b_ob =
4399 ((cartesian_trajectory_planner_B.smm1 +
4400 cartesian_trajectory_planner_B.rt) *
4401 (cartesian_trajectory_planner_B.smm1 -
4402 cartesian_trajectory_planner_B.rt) +
4403 cartesian_trajectory_planner_B.emm1 *
4404 cartesian_trajectory_planner_B.emm1) / 2.0;
4405 cartesian_trajectory_planner_B.smm1 = cartesian_trajectory_planner_B.rt *
4406 cartesian_trajectory_planner_B.emm1;
4407 cartesian_trajectory_planner_B.smm1 *= cartesian_trajectory_planner_B.smm1;
4408 if ((cartesian_trajectory_planner_B.b_ob != 0.0) ||
4409 (cartesian_trajectory_planner_B.smm1 != 0.0)) {
4410 cartesian_trajectory_planner_B.emm1 = sqrt
4411 (cartesian_trajectory_planner_B.b_ob *
4412 cartesian_trajectory_planner_B.b_ob +
4413 cartesian_trajectory_planner_B.smm1);
4414 if (cartesian_trajectory_planner_B.b_ob < 0.0) {
4415 cartesian_trajectory_planner_B.emm1 =
4416 -cartesian_trajectory_planner_B.emm1;
4417 }
4418
4419 cartesian_trajectory_planner_B.emm1 =
4420 cartesian_trajectory_planner_B.smm1 /
4421 (cartesian_trajectory_planner_B.b_ob +
4422 cartesian_trajectory_planner_B.emm1);
4423 } else {
4424 cartesian_trajectory_planner_B.emm1 = 0.0;
4425 }
4426
4427 cartesian_trajectory_planner_B.rt = (cartesian_trajectory_planner_B.sqds +
4428 cartesian_trajectory_planner_B.rt) *
4429 (cartesian_trajectory_planner_B.sqds - cartesian_trajectory_planner_B.rt)
4430 + cartesian_trajectory_planner_B.emm1;
4431 cartesian_trajectory_planner_B.sqds *=
4432 cartesian_trajectory_planner_B.e_j[c_q] /
4433 cartesian_trajectory_planner_B.ztest;
4434 for (d_k = c_q + 1; d_k <= m; d_k++) {
4435 cartesian_trajectory_plan_xrotg(cartesian_trajectory_planner_B.rt,
4436 cartesian_trajectory_planner_B.sqds,
4437 &cartesian_trajectory_planner_B.ztest,
4438 &cartesian_trajectory_planner_B.emm1,
4439 &cartesian_trajectory_planner_B.b_ob,
4440 &cartesian_trajectory_planner_B.smm1);
4441 if (d_k > c_q + 1) {
4442 cartesian_trajectory_planner_B.e_j[0] =
4443 cartesian_trajectory_planner_B.ztest;
4444 }
4445
4446 cartesian_trajectory_planner_B.ztest =
4447 cartesian_trajectory_planner_B.e_j[d_k - 1];
4448 cartesian_trajectory_planner_B.rt = cartesian_trajectory_planner_B.s[d_k
4449 - 1];
4450 cartesian_trajectory_planner_B.e_j[d_k - 1] =
4451 cartesian_trajectory_planner_B.ztest *
4452 cartesian_trajectory_planner_B.b_ob -
4453 cartesian_trajectory_planner_B.rt *
4454 cartesian_trajectory_planner_B.smm1;
4455 cartesian_trajectory_planner_B.sqds =
4456 cartesian_trajectory_planner_B.smm1 *
4457 cartesian_trajectory_planner_B.s[d_k];
4458 cartesian_trajectory_planner_B.s[d_k] *=
4459 cartesian_trajectory_planner_B.b_ob;
4460 qjj = (d_k - 1) * 3 + 1;
4461 kase = 3 * d_k + 1;
4462 memcpy(&cartesian_trajectory_planner_B.A[0], &V[0], 9U * sizeof(real_T));
4463 cartesian_trajectory_plann_xrot(cartesian_trajectory_planner_B.A, qjj,
4464 kase, cartesian_trajectory_planner_B.b_ob,
4465 cartesian_trajectory_planner_B.smm1, V);
4466 cartesian_trajectory_plan_xrotg(cartesian_trajectory_planner_B.rt *
4467 cartesian_trajectory_planner_B.b_ob +
4468 cartesian_trajectory_planner_B.ztest *
4469 cartesian_trajectory_planner_B.smm1,
4470 cartesian_trajectory_planner_B.sqds,
4471 &cartesian_trajectory_planner_B.s[d_k - 1],
4472 &cartesian_trajectory_planner_B.unusedU2,
4473 &cartesian_trajectory_planner_B.emm1,
4474 &cartesian_trajectory_planner_B.d_sn);
4475 cartesian_trajectory_planner_B.rt =
4476 cartesian_trajectory_planner_B.e_j[d_k - 1] *
4477 cartesian_trajectory_planner_B.emm1 +
4478 cartesian_trajectory_planner_B.d_sn *
4479 cartesian_trajectory_planner_B.s[d_k];
4480 cartesian_trajectory_planner_B.s[d_k] =
4481 cartesian_trajectory_planner_B.e_j[d_k - 1] *
4482 -cartesian_trajectory_planner_B.d_sn +
4483 cartesian_trajectory_planner_B.emm1 *
4484 cartesian_trajectory_planner_B.s[d_k];
4485 cartesian_trajectory_planner_B.sqds =
4486 cartesian_trajectory_planner_B.d_sn *
4487 cartesian_trajectory_planner_B.e_j[d_k];
4488 cartesian_trajectory_planner_B.e_j[d_k] *=
4489 cartesian_trajectory_planner_B.emm1;
4490 memcpy(&cartesian_trajectory_planner_B.A[0], &U[0], 9U * sizeof(real_T));
4491 cartesian_trajectory_plann_xrot(cartesian_trajectory_planner_B.A, qjj,
4492 kase, cartesian_trajectory_planner_B.emm1,
4493 cartesian_trajectory_planner_B.d_sn, U);
4494 }
4495
4496 cartesian_trajectory_planner_B.e_j[m - 1] =
4497 cartesian_trajectory_planner_B.rt;
4498 qq++;
4499 break;
4500
4501 default:
4502 if (cartesian_trajectory_planner_B.s[c_q] < 0.0) {
4503 cartesian_trajectory_planner_B.s[c_q] =
4504 -cartesian_trajectory_planner_B.s[c_q];
4505 qjj = 3 * c_q;
4506 for (qq = qjj + 1; qq <= qjj + 3; qq++) {
4507 V[qq - 1] = -V[qq - 1];
4508 }
4509 }
4510
4511 qq = c_q + 1;
4512 while ((c_q + 1 < 3) && (cartesian_trajectory_planner_B.s[c_q] <
4513 cartesian_trajectory_planner_B.s[qq])) {
4514 cartesian_trajectory_planner_B.rt = cartesian_trajectory_planner_B.s[c_q];
4515 cartesian_trajectory_planner_B.s[c_q] =
4516 cartesian_trajectory_planner_B.s[qq];
4517 cartesian_trajectory_planner_B.s[qq] = cartesian_trajectory_planner_B.rt;
4518 qjj = 3 * c_q + 1;
4519 kase = (c_q + 1) * 3 + 1;
4520 memcpy(&cartesian_trajectory_planner_B.A[0], &V[0], 9U * sizeof(real_T));
4521 cartesian_trajectory_plan_xswap(cartesian_trajectory_planner_B.A, qjj,
4522 kase, V);
4523 memcpy(&cartesian_trajectory_planner_B.A[0], &U[0], 9U * sizeof(real_T));
4524 cartesian_trajectory_plan_xswap(cartesian_trajectory_planner_B.A, qjj,
4525 kase, U);
4526 c_q = qq;
4527 qq++;
4528 }
4529
4530 qq = 0;
4531 m--;
4532 break;
4533 }
4534 }
4535
4536 s[0] = cartesian_trajectory_planner_B.s[0];
4537 s[1] = cartesian_trajectory_planner_B.s[1];
4538 s[2] = cartesian_trajectory_planner_B.s[2];
4539}
4540
4541static void cartesian_trajectory_rotm2axang(const real_T R[9], real_T axang[4])
4542{
4543 boolean_T e;
4544 boolean_T p;
4545 boolean_T rEQ0;
4546 int32_T loop_ub_tmp;
4547 boolean_T exitg1;
4548 cartesian_trajectory_planner_B.u.re = (((R[0] + R[4]) + R[8]) - 1.0) * 0.5;
4549 if (!(fabs(cartesian_trajectory_planner_B.u.re) > 1.0)) {
4550 cartesian_trajectory_planner_B.v_e.re = acos
4551 (cartesian_trajectory_planner_B.u.re);
4552 } else {
4553 cartesian_trajectory_planner_B.u_m.re = cartesian_trajectory_planner_B.u.re
4554 + 1.0;
4555 cartesian_trajectory_planner_B.u_m.im = 0.0;
4556 cartesian_trajectory_planner_B.dc.re = 1.0 -
4557 cartesian_trajectory_planner_B.u.re;
4558 cartesian_trajectory_planner_B.dc.im = 0.0;
4559 cartesian_trajectory_planner_B.v_e.re = 2.0 * rt_atan2d_snf
4560 ((cartesian_trajectory_plann_sqrt(cartesian_trajectory_planner_B.dc)).re,
4561 (cartesian_trajectory_plann_sqrt(cartesian_trajectory_planner_B.u_m)).re);
4562 }
4563
4564 cartesian_trajectory_planner_B.a_pi = 2.0 * sin
4565 (cartesian_trajectory_planner_B.v_e.re);
4566 cartesian_trajectory_planner_B.v_my[0] = (R[5] - R[7]) /
4567 cartesian_trajectory_planner_B.a_pi;
4568 cartesian_trajectory_planner_B.v_my[1] = (R[6] - R[2]) /
4569 cartesian_trajectory_planner_B.a_pi;
4570 cartesian_trajectory_planner_B.v_my[2] = (R[1] - R[3]) /
4571 cartesian_trajectory_planner_B.a_pi;
4572 if (rtIsNaN(cartesian_trajectory_planner_B.v_e.re) || rtIsInf
4573 (cartesian_trajectory_planner_B.v_e.re)) {
4574 cartesian_trajectory_planner_B.a_pi = (rtNaN);
4575 } else if (cartesian_trajectory_planner_B.v_e.re == 0.0) {
4576 cartesian_trajectory_planner_B.a_pi = 0.0;
4577 } else {
4578 cartesian_trajectory_planner_B.a_pi = fmod
4579 (cartesian_trajectory_planner_B.v_e.re, 3.1415926535897931);
4580 rEQ0 = (cartesian_trajectory_planner_B.a_pi == 0.0);
4581 if (!rEQ0) {
4582 cartesian_trajectory_planner_B.q = fabs
4583 (cartesian_trajectory_planner_B.v_e.re / 3.1415926535897931);
4584 rEQ0 = !(fabs(cartesian_trajectory_planner_B.q - floor
4585 (cartesian_trajectory_planner_B.q + 0.5)) >
4586 2.2204460492503131E-16 * cartesian_trajectory_planner_B.q);
4587 }
4588
4589 if (rEQ0) {
4590 cartesian_trajectory_planner_B.a_pi = 0.0;
4591 } else {
4592 if (cartesian_trajectory_planner_B.v_e.re < 0.0) {
4593 cartesian_trajectory_planner_B.a_pi += 3.1415926535897931;
4594 }
4595 }
4596 }
4597
4598 rEQ0 = (cartesian_trajectory_planner_B.a_pi == 0.0);
4599 e = true;
4600 cartesian_trajectory_planner_B.b_k_h = 0;
4601 exitg1 = false;
4602 while ((!exitg1) && (cartesian_trajectory_planner_B.b_k_h < 3)) {
4603 if (!(cartesian_trajectory_planner_B.v_my[cartesian_trajectory_planner_B.b_k_h]
4604 == 0.0)) {
4605 e = false;
4606 exitg1 = true;
4607 } else {
4608 cartesian_trajectory_planner_B.b_k_h++;
4609 }
4610 }
4611
4612 if (rEQ0 || e) {
4613 loop_ub_tmp = (rEQ0 || e);
4614 cartesian_trajectory_planner_B.loop_ub_j3 = loop_ub_tmp * 3 - 1;
4615 if (0 <= cartesian_trajectory_planner_B.loop_ub_j3) {
4616 memset(&cartesian_trajectory_planner_B.vspecial_data[0], 0,
4617 (cartesian_trajectory_planner_B.loop_ub_j3 + 1) * sizeof(real_T));
4618 }
4619
4620 loop_ub_tmp--;
4621 for (cartesian_trajectory_planner_B.loop_ub_j3 = 0;
4622 cartesian_trajectory_planner_B.loop_ub_j3 <= loop_ub_tmp;
4623 cartesian_trajectory_planner_B.loop_ub_j3++) {
4624 memset(&cartesian_trajectory_planner_B.b_I[0], 0, 9U * sizeof(real_T));
4625 cartesian_trajectory_planner_B.b_I[0] = 1.0;
4626 cartesian_trajectory_planner_B.b_I[4] = 1.0;
4627 cartesian_trajectory_planner_B.b_I[8] = 1.0;
4628 p = true;
4629 for (cartesian_trajectory_planner_B.b_k_h = 0;
4630 cartesian_trajectory_planner_B.b_k_h < 9;
4631 cartesian_trajectory_planner_B.b_k_h++) {
4632 cartesian_trajectory_planner_B.a_pi =
4633 cartesian_trajectory_planner_B.b_I[cartesian_trajectory_planner_B.b_k_h]
4634 - R[cartesian_trajectory_planner_B.b_k_h];
4635 if (p && ((!rtIsInf(cartesian_trajectory_planner_B.a_pi)) && (!rtIsNaN
4636 (cartesian_trajectory_planner_B.a_pi)))) {
4637 } else {
4638 p = false;
4639 }
4640
4641 cartesian_trajectory_planner_B.b_I[cartesian_trajectory_planner_B.b_k_h]
4642 = cartesian_trajectory_planner_B.a_pi;
4643 }
4644
4645 if (p) {
4646 cartesian_trajectory_planne_svd(cartesian_trajectory_planner_B.b_I,
4647 cartesian_trajectory_planner_B.b_U,
4648 cartesian_trajectory_planner_B.vspecial_data,
4649 cartesian_trajectory_planner_B.V_p);
4650 } else {
4651 for (cartesian_trajectory_planner_B.b_k_h = 0;
4652 cartesian_trajectory_planner_B.b_k_h < 9;
4653 cartesian_trajectory_planner_B.b_k_h++) {
4654 cartesian_trajectory_planner_B.V_p[cartesian_trajectory_planner_B.b_k_h]
4655 = (rtNaN);
4656 }
4657 }
4658
4659 cartesian_trajectory_planner_B.vspecial_data[0] =
4660 cartesian_trajectory_planner_B.V_p[6];
4661 cartesian_trajectory_planner_B.vspecial_data[1] =
4662 cartesian_trajectory_planner_B.V_p[7];
4663 cartesian_trajectory_planner_B.vspecial_data[2] =
4664 cartesian_trajectory_planner_B.V_p[8];
4665 }
4666
4667 loop_ub_tmp = 0;
4668 if (rEQ0 || e) {
4669 for (cartesian_trajectory_planner_B.loop_ub_j3 = 0;
4670 cartesian_trajectory_planner_B.loop_ub_j3 < 1;
4671 cartesian_trajectory_planner_B.loop_ub_j3++) {
4672 loop_ub_tmp++;
4673 }
4674 }
4675
4676 for (cartesian_trajectory_planner_B.b_k_h = 0;
4677 cartesian_trajectory_planner_B.b_k_h < loop_ub_tmp;
4678 cartesian_trajectory_planner_B.b_k_h++) {
4679 cartesian_trajectory_planner_B.v_my[0] =
4680 cartesian_trajectory_planner_B.vspecial_data[3 *
4681 cartesian_trajectory_planner_B.b_k_h];
4682 cartesian_trajectory_planner_B.v_my[1] =
4683 cartesian_trajectory_planner_B.vspecial_data[3 *
4684 cartesian_trajectory_planner_B.b_k_h + 1];
4685 cartesian_trajectory_planner_B.v_my[2] =
4686 cartesian_trajectory_planner_B.vspecial_data[3 *
4687 cartesian_trajectory_planner_B.b_k_h + 2];
4688 }
4689 }
4690
4691 cartesian_trajectory_planner_B.a_pi = 1.0 / sqrt
4692 ((cartesian_trajectory_planner_B.v_my[0] *
4693 cartesian_trajectory_planner_B.v_my[0] +
4694 cartesian_trajectory_planner_B.v_my[1] *
4695 cartesian_trajectory_planner_B.v_my[1]) +
4696 cartesian_trajectory_planner_B.v_my[2] *
4697 cartesian_trajectory_planner_B.v_my[2]);
4698 cartesian_trajectory_planner_B.v_my[0] *= cartesian_trajectory_planner_B.a_pi;
4699 cartesian_trajectory_planner_B.v_my[1] *= cartesian_trajectory_planner_B.a_pi;
4700 axang[0] = cartesian_trajectory_planner_B.v_my[0];
4701 axang[1] = cartesian_trajectory_planner_B.v_my[1];
4702 axang[2] = cartesian_trajectory_planner_B.v_my[2] *
4703 cartesian_trajectory_planner_B.a_pi;
4704 axang[3] = cartesian_trajectory_planner_B.v_e.re;
4705}
4706
4707static void cartesian_IKHelpers_computeCost(const real_T x[6],
4708 f_robotics_manip_internal_IKE_T *args, real_T *cost, real_T W[36],
4709 emxArray_real_T_cartesian_tra_T *Jac, f_robotics_manip_internal_IKE_T **b_args)
4710{
4711 x_robotics_manip_internal_Rig_T *treeInternal;
4712 emxArray_char_T_cartesian_tra_T *bodyName;
4713 emxArray_real_T_cartesian_tra_T *J;
4714 emxArray_real_T_cartesian_tra_T *y;
4715 cartesian_trajec_emxInit_char_T(&bodyName, 2);
4716 *b_args = args;
4717 treeInternal = args->Robot;
4718 cartesian_trajectory_planner_B.b_j_h = bodyName->size[0] * bodyName->size[1];
4719 bodyName->size[0] = 1;
4720 bodyName->size[1] = args->BodyName->size[1];
4721 cartes_emxEnsureCapacity_char_T(bodyName, cartesian_trajectory_planner_B.b_j_h);
4722 cartesian_trajectory_planner_B.loop_ub_f = args->BodyName->size[0] *
4723 args->BodyName->size[1] - 1;
4724 for (cartesian_trajectory_planner_B.b_j_h = 0;
4725 cartesian_trajectory_planner_B.b_j_h <=
4726 cartesian_trajectory_planner_B.loop_ub_f;
4727 cartesian_trajectory_planner_B.b_j_h++) {
4728 bodyName->data[cartesian_trajectory_planner_B.b_j_h] = args->BodyName->
4729 data[cartesian_trajectory_planner_B.b_j_h];
4730 }
4731
4732 for (cartesian_trajectory_planner_B.b_j_h = 0;
4733 cartesian_trajectory_planner_B.b_j_h < 16;
4734 cartesian_trajectory_planner_B.b_j_h++) {
4735 cartesian_trajectory_planner_B.Td[cartesian_trajectory_planner_B.b_j_h] =
4736 args->Tform[cartesian_trajectory_planner_B.b_j_h];
4737 }
4738
4739 for (cartesian_trajectory_planner_B.b_j_h = 0;
4740 cartesian_trajectory_planner_B.b_j_h < 36;
4741 cartesian_trajectory_planner_B.b_j_h++) {
4742 W[cartesian_trajectory_planner_B.b_j_h] = args->
4743 WeightMatrix[cartesian_trajectory_planner_B.b_j_h];
4744 }
4745
4746 cartesian_trajec_emxInit_real_T(&J, 2);
4747 RigidBodyTree_efficientFKAndJac(treeInternal, x, bodyName,
4748 cartesian_trajectory_planner_B.T_data, cartesian_trajectory_planner_B.T_size,
4749 J);
4750 cartesian_trajectory_planner_B.b_j_h = Jac->size[0] * Jac->size[1];
4751 Jac->size[0] = 6;
4752 Jac->size[1] = J->size[1];
4753 cartes_emxEnsureCapacity_real_T(Jac, cartesian_trajectory_planner_B.b_j_h);
4754 cartesian_trajectory_planner_B.loop_ub_f = J->size[0] * J->size[1] - 1;
4755 cartesian_trajec_emxFree_char_T(&bodyName);
4756 for (cartesian_trajectory_planner_B.b_j_h = 0;
4757 cartesian_trajectory_planner_B.b_j_h <=
4758 cartesian_trajectory_planner_B.loop_ub_f;
4759 cartesian_trajectory_planner_B.b_j_h++) {
4760 Jac->data[cartesian_trajectory_planner_B.b_j_h] = -J->
4761 data[cartesian_trajectory_planner_B.b_j_h];
4762 }
4763
4764 cartesian_trajec_emxFree_real_T(&J);
4765 for (cartesian_trajectory_planner_B.b_j_h = 0;
4766 cartesian_trajectory_planner_B.b_j_h < 3;
4767 cartesian_trajectory_planner_B.b_j_h++) {
4768 cartesian_trajectory_planner_B.T_o[3 * cartesian_trajectory_planner_B.b_j_h]
4769 =
4770 cartesian_trajectory_planner_B.T_data[cartesian_trajectory_planner_B.b_j_h];
4771 cartesian_trajectory_planner_B.n_n = 3 *
4772 cartesian_trajectory_planner_B.b_j_h + 1;
4773 cartesian_trajectory_planner_B.T_o[cartesian_trajectory_planner_B.n_n] =
4774 cartesian_trajectory_planner_B.T_data
4775 [((cartesian_trajectory_planner_B.b_j_h + 1) +
4776 cartesian_trajectory_planner_B.T_size[0]) - 1];
4777 cartesian_trajectory_planner_B.boffset_h = 3 *
4778 cartesian_trajectory_planner_B.b_j_h + 2;
4779 cartesian_trajectory_planner_B.T_o[cartesian_trajectory_planner_B.boffset_h]
4780 = cartesian_trajectory_planner_B.T_data
4781 [((cartesian_trajectory_planner_B.b_j_h + 1) +
4782 (cartesian_trajectory_planner_B.T_size[0] << 1)) - 1];
4783 for (cartesian_trajectory_planner_B.loop_ub_f = 0;
4784 cartesian_trajectory_planner_B.loop_ub_f < 3;
4785 cartesian_trajectory_planner_B.loop_ub_f++) {
4786 cartesian_trajectory_planner_B.Td_tmp =
4787 cartesian_trajectory_planner_B.loop_ub_f + 3 *
4788 cartesian_trajectory_planner_B.b_j_h;
4789 cartesian_trajectory_planner_B.Td_o[cartesian_trajectory_planner_B.Td_tmp]
4790 = 0.0;
4791 cartesian_trajectory_planner_B.Td_o[cartesian_trajectory_planner_B.Td_tmp]
4792 += cartesian_trajectory_planner_B.T_o[3 *
4793 cartesian_trajectory_planner_B.b_j_h] *
4794 cartesian_trajectory_planner_B.Td[cartesian_trajectory_planner_B.loop_ub_f];
4795 cartesian_trajectory_planner_B.Td_o[cartesian_trajectory_planner_B.Td_tmp]
4796 += cartesian_trajectory_planner_B.T_o[cartesian_trajectory_planner_B.n_n]
4797 * cartesian_trajectory_planner_B.Td[cartesian_trajectory_planner_B.loop_ub_f
4798 + 4];
4799 cartesian_trajectory_planner_B.Td_o[cartesian_trajectory_planner_B.Td_tmp]
4800 +=
4801 cartesian_trajectory_planner_B.T_o[cartesian_trajectory_planner_B.boffset_h]
4802 * cartesian_trajectory_planner_B.Td[cartesian_trajectory_planner_B.loop_ub_f
4803 + 8];
4804 }
4805 }
4806
4807 cartesian_trajectory_rotm2axang(cartesian_trajectory_planner_B.Td_o,
4808 cartesian_trajectory_planner_B.v);
4809 cartesian_trajectory_planner_B.e[0] = cartesian_trajectory_planner_B.v[3] *
4810 cartesian_trajectory_planner_B.v[0];
4811 cartesian_trajectory_planner_B.e[3] = cartesian_trajectory_planner_B.Td[12] -
4812 cartesian_trajectory_planner_B.T_data[cartesian_trajectory_planner_B.T_size
4813 [0] * 3];
4814 cartesian_trajectory_planner_B.e[1] = cartesian_trajectory_planner_B.v[3] *
4815 cartesian_trajectory_planner_B.v[1];
4816 cartesian_trajectory_planner_B.e[4] = cartesian_trajectory_planner_B.Td[13] -
4817 cartesian_trajectory_planner_B.T_data[cartesian_trajectory_planner_B.T_size
4818 [0] * 3 + 1];
4819 cartesian_trajectory_planner_B.e[2] = cartesian_trajectory_planner_B.v[3] *
4820 cartesian_trajectory_planner_B.v[2];
4821 cartesian_trajectory_planner_B.e[5] = cartesian_trajectory_planner_B.Td[14] -
4822 cartesian_trajectory_planner_B.T_data[cartesian_trajectory_planner_B.T_size
4823 [0] * 3 + 2];
4824 cartesian_trajectory_planner_B.b_j_h = args->ErrTemp->size[0];
4825 args->ErrTemp->size[0] = 6;
4826 cartes_emxEnsureCapacity_real_T(args->ErrTemp,
4827 cartesian_trajectory_planner_B.b_j_h);
4828 for (cartesian_trajectory_planner_B.b_j_h = 0;
4829 cartesian_trajectory_planner_B.b_j_h < 6;
4830 cartesian_trajectory_planner_B.b_j_h++) {
4831 args->ErrTemp->data[cartesian_trajectory_planner_B.b_j_h] =
4832 cartesian_trajectory_planner_B.e[cartesian_trajectory_planner_B.b_j_h];
4833 }
4834
4835 for (cartesian_trajectory_planner_B.b_j_h = 0;
4836 cartesian_trajectory_planner_B.b_j_h < 6;
4837 cartesian_trajectory_planner_B.b_j_h++) {
4838 cartesian_trajectory_planner_B.y[cartesian_trajectory_planner_B.b_j_h] = 0.0;
4839 for (cartesian_trajectory_planner_B.loop_ub_f = 0;
4840 cartesian_trajectory_planner_B.loop_ub_f < 6;
4841 cartesian_trajectory_planner_B.loop_ub_f++) {
4842 cartesian_trajectory_planner_B.s_n = W[6 *
4843 cartesian_trajectory_planner_B.b_j_h +
4844 cartesian_trajectory_planner_B.loop_ub_f] * (0.5 *
4845 cartesian_trajectory_planner_B.e[cartesian_trajectory_planner_B.loop_ub_f])
4846 + cartesian_trajectory_planner_B.y[cartesian_trajectory_planner_B.b_j_h];
4847 cartesian_trajectory_planner_B.y[cartesian_trajectory_planner_B.b_j_h] =
4848 cartesian_trajectory_planner_B.s_n;
4849 }
4850 }
4851
4852 cartesian_trajectory_planner_B.s_n = 0.0;
4853 for (cartesian_trajectory_planner_B.b_j_h = 0;
4854 cartesian_trajectory_planner_B.b_j_h < 6;
4855 cartesian_trajectory_planner_B.b_j_h++) {
4856 cartesian_trajectory_planner_B.s_n +=
4857 cartesian_trajectory_planner_B.y[cartesian_trajectory_planner_B.b_j_h] *
4858 cartesian_trajectory_planner_B.e[cartesian_trajectory_planner_B.b_j_h];
4859 }
4860
4861 args->CostTemp = cartesian_trajectory_planner_B.s_n;
4862 for (cartesian_trajectory_planner_B.b_j_h = 0;
4863 cartesian_trajectory_planner_B.b_j_h < 6;
4864 cartesian_trajectory_planner_B.b_j_h++) {
4865 cartesian_trajectory_planner_B.y[cartesian_trajectory_planner_B.b_j_h] = 0.0;
4866 for (cartesian_trajectory_planner_B.loop_ub_f = 0;
4867 cartesian_trajectory_planner_B.loop_ub_f < 6;
4868 cartesian_trajectory_planner_B.loop_ub_f++) {
4869 cartesian_trajectory_planner_B.s_n = W[6 *
4870 cartesian_trajectory_planner_B.b_j_h +
4871 cartesian_trajectory_planner_B.loop_ub_f] *
4872 cartesian_trajectory_planner_B.e[cartesian_trajectory_planner_B.loop_ub_f]
4873 + cartesian_trajectory_planner_B.y[cartesian_trajectory_planner_B.b_j_h];
4874 cartesian_trajectory_planner_B.y[cartesian_trajectory_planner_B.b_j_h] =
4875 cartesian_trajectory_planner_B.s_n;
4876 }
4877 }
4878
4879 cartesian_trajec_emxInit_real_T(&y, 2);
4880 cartesian_trajectory_planner_B.n_n = Jac->size[1] - 1;
4881 cartesian_trajectory_planner_B.b_j_h = y->size[0] * y->size[1];
4882 y->size[0] = 1;
4883 y->size[1] = Jac->size[1];
4884 cartes_emxEnsureCapacity_real_T(y, cartesian_trajectory_planner_B.b_j_h);
4885 for (cartesian_trajectory_planner_B.b_j_h = 0;
4886 cartesian_trajectory_planner_B.b_j_h <=
4887 cartesian_trajectory_planner_B.n_n; cartesian_trajectory_planner_B.b_j_h
4888 ++) {
4889 cartesian_trajectory_planner_B.boffset_h =
4890 cartesian_trajectory_planner_B.b_j_h * 6 - 1;
4891 cartesian_trajectory_planner_B.s_n = 0.0;
4892 for (cartesian_trajectory_planner_B.loop_ub_f = 0;
4893 cartesian_trajectory_planner_B.loop_ub_f < 6;
4894 cartesian_trajectory_planner_B.loop_ub_f++) {
4895 cartesian_trajectory_planner_B.s_n += Jac->data
4896 [(cartesian_trajectory_planner_B.boffset_h +
4897 cartesian_trajectory_planner_B.loop_ub_f) + 1] *
4898 cartesian_trajectory_planner_B.y[cartesian_trajectory_planner_B.loop_ub_f];
4899 }
4900
4901 y->data[cartesian_trajectory_planner_B.b_j_h] =
4902 cartesian_trajectory_planner_B.s_n;
4903 }
4904
4905 cartesian_trajectory_planner_B.b_j_h = args->GradTemp->size[0];
4906 args->GradTemp->size[0] = y->size[1];
4907 cartes_emxEnsureCapacity_real_T(args->GradTemp,
4908 cartesian_trajectory_planner_B.b_j_h);
4909 cartesian_trajectory_planner_B.loop_ub_f = y->size[1];
4910 for (cartesian_trajectory_planner_B.b_j_h = 0;
4911 cartesian_trajectory_planner_B.b_j_h <
4912 cartesian_trajectory_planner_B.loop_ub_f;
4913 cartesian_trajectory_planner_B.b_j_h++) {
4914 args->GradTemp->data[cartesian_trajectory_planner_B.b_j_h] = y->
4915 data[cartesian_trajectory_planner_B.b_j_h];
4916 }
4917
4918 cartesian_trajec_emxFree_real_T(&y);
4919 cartesian_trajectory_planner_B.s_n = args->CostTemp;
4920 *cost = cartesian_trajectory_planner_B.s_n;
4921}
4922
4923static void cartesian_trajectory_planne_eye(real_T b_I[36])
4924{
4925 int32_T b_k;
4926 memset(&b_I[0], 0, 36U * sizeof(real_T));
4927 for (b_k = 0; b_k < 6; b_k++) {
4928 b_I[b_k + 6 * b_k] = 1.0;
4929 }
4930}
4931
4932static void cartesian_tra_emxInit_boolean_T(emxArray_boolean_T_cartesian__T
4933 **pEmxArray, int32_T numDimensions)
4934{
4935 emxArray_boolean_T_cartesian__T *emxArray;
4936 int32_T i;
4937 *pEmxArray = (emxArray_boolean_T_cartesian__T *)malloc(sizeof
4938 (emxArray_boolean_T_cartesian__T));
4939 emxArray = *pEmxArray;
4940 emxArray->data = (boolean_T *)NULL;
4941 emxArray->numDimensions = numDimensions;
4942 emxArray->size = (int32_T *)malloc(sizeof(int32_T) * numDimensions);
4943 emxArray->allocatedSize = 0;
4944 emxArray->canFreeData = true;
4945 for (i = 0; i < numDimensions; i++) {
4946 emxArray->size[i] = 0;
4947 }
4948}
4949
4950static void cartesian_traje_emxInit_int32_T(emxArray_int32_T_cartesian_tr_T
4951 **pEmxArray, int32_T numDimensions)
4952{
4953 emxArray_int32_T_cartesian_tr_T *emxArray;
4954 int32_T i;
4955 *pEmxArray = (emxArray_int32_T_cartesian_tr_T *)malloc(sizeof
4956 (emxArray_int32_T_cartesian_tr_T));
4957 emxArray = *pEmxArray;
4958 emxArray->data = (int32_T *)NULL;
4959 emxArray->numDimensions = numDimensions;
4960 emxArray->size = (int32_T *)malloc(sizeof(int32_T) * numDimensions);
4961 emxArray->allocatedSize = 0;
4962 emxArray->canFreeData = true;
4963 for (i = 0; i < numDimensions; i++) {
4964 emxArray->size[i] = 0;
4965 }
4966}
4967
4968static void car_emxEnsureCapacity_boolean_T(emxArray_boolean_T_cartesian__T
4969 *emxArray, int32_T oldNumel)
4970{
4971 int32_T newNumel;
4972 int32_T i;
4973 void *newData;
4974 if (oldNumel < 0) {
4975 oldNumel = 0;
4976 }
4977
4978 newNumel = 1;
4979 for (i = 0; i < emxArray->numDimensions; i++) {
4980 newNumel *= emxArray->size[i];
4981 }
4982
4983 if (newNumel > emxArray->allocatedSize) {
4984 i = emxArray->allocatedSize;
4985 if (i < 16) {
4986 i = 16;
4987 }
4988
4989 while (i < newNumel) {
4990 if (i > 1073741823) {
4991 i = MAX_int32_T;
4992 } else {
4993 i <<= 1;
4994 }
4995 }
4996
4997 newData = calloc(static_cast<uint32_T>(i), sizeof(boolean_T));
4998 if (emxArray->data != NULL) {
4999 memcpy(newData, emxArray->data, sizeof(boolean_T) * oldNumel);
5000 if (emxArray->canFreeData) {
5001 free(emxArray->data);
5002 }
5003 }
5004
5005 emxArray->data = (boolean_T *)newData;
5006 emxArray->allocatedSize = i;
5007 emxArray->canFreeData = true;
5008 }
5009}
5010
5011static void carte_emxEnsureCapacity_int32_T(emxArray_int32_T_cartesian_tr_T
5012 *emxArray, int32_T oldNumel)
5013{
5014 int32_T newNumel;
5015 int32_T i;
5016 void *newData;
5017 if (oldNumel < 0) {
5018 oldNumel = 0;
5019 }
5020
5021 newNumel = 1;
5022 for (i = 0; i < emxArray->numDimensions; i++) {
5023 newNumel *= emxArray->size[i];
5024 }
5025
5026 if (newNumel > emxArray->allocatedSize) {
5027 i = emxArray->allocatedSize;
5028 if (i < 16) {
5029 i = 16;
5030 }
5031
5032 while (i < newNumel) {
5033 if (i > 1073741823) {
5034 i = MAX_int32_T;
5035 } else {
5036 i <<= 1;
5037 }
5038 }
5039
5040 newData = calloc(static_cast<uint32_T>(i), sizeof(int32_T));
5041 if (emxArray->data != NULL) {
5042 memcpy(newData, emxArray->data, sizeof(int32_T) * oldNumel);
5043 if (emxArray->canFreeData) {
5044 free(emxArray->data);
5045 }
5046 }
5047
5048 emxArray->data = (int32_T *)newData;
5049 emxArray->allocatedSize = i;
5050 emxArray->canFreeData = true;
5051 }
5052}
5053
5054static real_T cartesian_trajectory_pla_norm_a(const real_T x[6])
5055{
5056 real_T y;
5057 real_T scale;
5058 real_T absxk;
5059 real_T t;
5060 int32_T b_k;
5061 y = 0.0;
5062 scale = 3.3121686421112381E-170;
5063 for (b_k = 0; b_k < 6; b_k++) {
5064 absxk = fabs(x[b_k]);
5065 if (absxk > scale) {
5066 t = scale / absxk;
5067 y = y * t * t + 1.0;
5068 scale = absxk;
5069 } else {
5070 t = absxk / scale;
5071 y += t * t;
5072 }
5073 }
5074
5075 return scale * sqrt(y);
5076}
5077
5078static real_T SystemTimeProvider_getElapsedTi(const
5079 f_robotics_core_internal_Syst_T *obj)
5080{
5081 real_T systemTime;
5082 systemTime = ctimefun();
5083 return systemTime - obj->StartTime;
5084}
5085
5086static real_T cartesian_trajectory_p_xnrm2_as(int32_T n, const
5087 emxArray_real_T_cartesian_tra_T *x, int32_T ix0)
5088{
5089 real_T y;
5090 y = 0.0;
5091 if (n >= 1) {
5092 if (n == 1) {
5093 y = fabs(x->data[ix0 - 1]);
5094 } else {
5095 cartesian_trajectory_planner_B.scale_j = 3.3121686421112381E-170;
5096 cartesian_trajectory_planner_B.kend_f = ix0 + n;
5097 for (cartesian_trajectory_planner_B.k_i = ix0;
5098 cartesian_trajectory_planner_B.k_i <
5099 cartesian_trajectory_planner_B.kend_f;
5100 cartesian_trajectory_planner_B.k_i++) {
5101 cartesian_trajectory_planner_B.absxk_o = fabs(x->
5102 data[cartesian_trajectory_planner_B.k_i - 1]);
5103 if (cartesian_trajectory_planner_B.absxk_o >
5104 cartesian_trajectory_planner_B.scale_j) {
5105 cartesian_trajectory_planner_B.t_f =
5106 cartesian_trajectory_planner_B.scale_j /
5107 cartesian_trajectory_planner_B.absxk_o;
5108 y = y * cartesian_trajectory_planner_B.t_f *
5109 cartesian_trajectory_planner_B.t_f + 1.0;
5110 cartesian_trajectory_planner_B.scale_j =
5111 cartesian_trajectory_planner_B.absxk_o;
5112 } else {
5113 cartesian_trajectory_planner_B.t_f =
5114 cartesian_trajectory_planner_B.absxk_o /
5115 cartesian_trajectory_planner_B.scale_j;
5116 y += cartesian_trajectory_planner_B.t_f *
5117 cartesian_trajectory_planner_B.t_f;
5118 }
5119 }
5120
5121 y = cartesian_trajectory_planner_B.scale_j * sqrt(y);
5122 }
5123 }
5124
5125 return y;
5126}
5127
5128static void cartesian_trajectory_pla_qrpf_a(const
5129 emxArray_real_T_cartesian_tra_T *A, int32_T m, int32_T n,
5130 emxArray_real_T_cartesian_tra_T *tau, const emxArray_int32_T_cartesian_tr_T
5131 *jpvt, emxArray_real_T_cartesian_tra_T *b_A, emxArray_int32_T_cartesian_tr_T
5132 *b_jpvt)
5133{
5134 emxArray_real_T_cartesian_tra_T *work;
5135 emxArray_real_T_cartesian_tra_T *vn1;
5136 emxArray_real_T_cartesian_tra_T *vn2;
5137 emxArray_real_T_cartesian_tra_T *c_x;
5138 int32_T exitg1;
5139 boolean_T exitg2;
5140 cartesian_trajectory_planner_B.kend = b_jpvt->size[0] * b_jpvt->size[1];
5141 b_jpvt->size[0] = 1;
5142 b_jpvt->size[1] = jpvt->size[1];
5143 carte_emxEnsureCapacity_int32_T(b_jpvt, cartesian_trajectory_planner_B.kend);
5144 cartesian_trajectory_planner_B.ix_e = jpvt->size[0] * jpvt->size[1] - 1;
5145 for (cartesian_trajectory_planner_B.kend = 0;
5146 cartesian_trajectory_planner_B.kend <=
5147 cartesian_trajectory_planner_B.ix_e; cartesian_trajectory_planner_B.kend
5148 ++) {
5149 b_jpvt->data[cartesian_trajectory_planner_B.kend] = jpvt->
5150 data[cartesian_trajectory_planner_B.kend];
5151 }
5152
5153 cartesian_trajectory_planner_B.kend = b_A->size[0] * b_A->size[1];
5154 b_A->size[0] = A->size[0];
5155 b_A->size[1] = A->size[1];
5156 cartes_emxEnsureCapacity_real_T(b_A, cartesian_trajectory_planner_B.kend);
5157 cartesian_trajectory_planner_B.ix_e = A->size[0] * A->size[1] - 1;
5158 for (cartesian_trajectory_planner_B.kend = 0;
5159 cartesian_trajectory_planner_B.kend <=
5160 cartesian_trajectory_planner_B.ix_e; cartesian_trajectory_planner_B.kend
5161 ++) {
5162 b_A->data[cartesian_trajectory_planner_B.kend] = A->
5163 data[cartesian_trajectory_planner_B.kend];
5164 }
5165
5166 cartesian_trajec_emxInit_real_T(&work, 1);
5167 cartesian_trajectory_planner_B.ma = A->size[0];
5168 if (m < n) {
5169 cartesian_trajectory_planner_B.m_nq = m;
5170 } else {
5171 cartesian_trajectory_planner_B.m_nq = n;
5172 }
5173
5174 cartesian_trajectory_planner_B.minmn_n = cartesian_trajectory_planner_B.m_nq -
5175 1;
5176 cartesian_trajectory_planner_B.kend = work->size[0];
5177 work->size[0] = A->size[1];
5178 cartes_emxEnsureCapacity_real_T(work, cartesian_trajectory_planner_B.kend);
5179 cartesian_trajectory_planner_B.ix_e = A->size[1];
5180 for (cartesian_trajectory_planner_B.kend = 0;
5181 cartesian_trajectory_planner_B.kend < cartesian_trajectory_planner_B.ix_e;
5182 cartesian_trajectory_planner_B.kend++) {
5183 work->data[cartesian_trajectory_planner_B.kend] = 0.0;
5184 }
5185
5186 cartesian_trajec_emxInit_real_T(&vn1, 1);
5187 cartesian_trajectory_planner_B.kend = vn1->size[0];
5188 vn1->size[0] = A->size[1];
5189 cartes_emxEnsureCapacity_real_T(vn1, cartesian_trajectory_planner_B.kend);
5190 cartesian_trajectory_planner_B.ix_e = A->size[1];
5191 for (cartesian_trajectory_planner_B.kend = 0;
5192 cartesian_trajectory_planner_B.kend < cartesian_trajectory_planner_B.ix_e;
5193 cartesian_trajectory_planner_B.kend++) {
5194 vn1->data[cartesian_trajectory_planner_B.kend] = 0.0;
5195 }
5196
5197 cartesian_trajec_emxInit_real_T(&vn2, 1);
5198 cartesian_trajectory_planner_B.kend = vn2->size[0];
5199 vn2->size[0] = A->size[1];
5200 cartes_emxEnsureCapacity_real_T(vn2, cartesian_trajectory_planner_B.kend);
5201 cartesian_trajectory_planner_B.ix_e = A->size[1];
5202 for (cartesian_trajectory_planner_B.kend = 0;
5203 cartesian_trajectory_planner_B.kend < cartesian_trajectory_planner_B.ix_e;
5204 cartesian_trajectory_planner_B.kend++) {
5205 vn2->data[cartesian_trajectory_planner_B.kend] = 0.0;
5206 }
5207
5208 for (cartesian_trajectory_planner_B.m_nq = 0;
5209 cartesian_trajectory_planner_B.m_nq < n;
5210 cartesian_trajectory_planner_B.m_nq++) {
5211 cartesian_trajectory_planner_B.pvt = cartesian_trajectory_planner_B.m_nq *
5212 cartesian_trajectory_planner_B.ma;
5213 cartesian_trajectory_planner_B.smax = 0.0;
5214 if (m >= 1) {
5215 if (m == 1) {
5216 cartesian_trajectory_planner_B.smax = fabs(A->
5217 data[cartesian_trajectory_planner_B.pvt]);
5218 } else {
5219 cartesian_trajectory_planner_B.scale_f = 3.3121686421112381E-170;
5220 cartesian_trajectory_planner_B.kend = cartesian_trajectory_planner_B.pvt
5221 + m;
5222 for (cartesian_trajectory_planner_B.itemp =
5223 cartesian_trajectory_planner_B.pvt + 1;
5224 cartesian_trajectory_planner_B.itemp <=
5225 cartesian_trajectory_planner_B.kend;
5226 cartesian_trajectory_planner_B.itemp++) {
5227 cartesian_trajectory_planner_B.absxk = fabs(A->
5228 data[cartesian_trajectory_planner_B.itemp - 1]);
5229 if (cartesian_trajectory_planner_B.absxk >
5230 cartesian_trajectory_planner_B.scale_f) {
5231 cartesian_trajectory_planner_B.t =
5232 cartesian_trajectory_planner_B.scale_f /
5233 cartesian_trajectory_planner_B.absxk;
5234 cartesian_trajectory_planner_B.smax =
5235 cartesian_trajectory_planner_B.smax *
5236 cartesian_trajectory_planner_B.t *
5237 cartesian_trajectory_planner_B.t + 1.0;
5238 cartesian_trajectory_planner_B.scale_f =
5239 cartesian_trajectory_planner_B.absxk;
5240 } else {
5241 cartesian_trajectory_planner_B.t =
5242 cartesian_trajectory_planner_B.absxk /
5243 cartesian_trajectory_planner_B.scale_f;
5244 cartesian_trajectory_planner_B.smax +=
5245 cartesian_trajectory_planner_B.t *
5246 cartesian_trajectory_planner_B.t;
5247 }
5248 }
5249
5250 cartesian_trajectory_planner_B.smax =
5251 cartesian_trajectory_planner_B.scale_f * sqrt
5252 (cartesian_trajectory_planner_B.smax);
5253 }
5254 }
5255
5256 vn1->data[cartesian_trajectory_planner_B.m_nq] =
5257 cartesian_trajectory_planner_B.smax;
5258 vn2->data[cartesian_trajectory_planner_B.m_nq] = vn1->
5259 data[cartesian_trajectory_planner_B.m_nq];
5260 }
5261
5262 cartesian_trajec_emxInit_real_T(&c_x, 2);
5263 for (cartesian_trajectory_planner_B.m_nq = 0;
5264 cartesian_trajectory_planner_B.m_nq <=
5265 cartesian_trajectory_planner_B.minmn_n;
5266 cartesian_trajectory_planner_B.m_nq++) {
5267 cartesian_trajectory_planner_B.iy_b = cartesian_trajectory_planner_B.m_nq *
5268 cartesian_trajectory_planner_B.ma;
5269 cartesian_trajectory_planner_B.ii = cartesian_trajectory_planner_B.iy_b +
5270 cartesian_trajectory_planner_B.m_nq;
5271 cartesian_trajectory_planner_B.nmi = n - cartesian_trajectory_planner_B.m_nq;
5272 cartesian_trajectory_planner_B.mmi = (m -
5273 cartesian_trajectory_planner_B.m_nq) - 1;
5274 if (cartesian_trajectory_planner_B.nmi < 1) {
5275 cartesian_trajectory_planner_B.kend = 0;
5276 } else {
5277 cartesian_trajectory_planner_B.kend = 1;
5278 if (cartesian_trajectory_planner_B.nmi > 1) {
5279 cartesian_trajectory_planner_B.ix_e =
5280 cartesian_trajectory_planner_B.m_nq;
5281 cartesian_trajectory_planner_B.smax = fabs(vn1->
5282 data[cartesian_trajectory_planner_B.m_nq]);
5283 for (cartesian_trajectory_planner_B.itemp = 2;
5284 cartesian_trajectory_planner_B.itemp <=
5285 cartesian_trajectory_planner_B.nmi;
5286 cartesian_trajectory_planner_B.itemp++) {
5287 cartesian_trajectory_planner_B.ix_e++;
5288 cartesian_trajectory_planner_B.scale_f = fabs(vn1->
5289 data[cartesian_trajectory_planner_B.ix_e]);
5290 if (cartesian_trajectory_planner_B.scale_f >
5291 cartesian_trajectory_planner_B.smax) {
5292 cartesian_trajectory_planner_B.kend =
5293 cartesian_trajectory_planner_B.itemp;
5294 cartesian_trajectory_planner_B.smax =
5295 cartesian_trajectory_planner_B.scale_f;
5296 }
5297 }
5298 }
5299 }
5300
5301 cartesian_trajectory_planner_B.pvt = (cartesian_trajectory_planner_B.m_nq +
5302 cartesian_trajectory_planner_B.kend) - 1;
5303 if (cartesian_trajectory_planner_B.pvt + 1 !=
5304 cartesian_trajectory_planner_B.m_nq + 1) {
5305 cartesian_trajectory_planner_B.kend = c_x->size[0] * c_x->size[1];
5306 c_x->size[0] = b_A->size[0];
5307 c_x->size[1] = b_A->size[1];
5308 cartes_emxEnsureCapacity_real_T(c_x, cartesian_trajectory_planner_B.kend);
5309 cartesian_trajectory_planner_B.ix_e = b_A->size[0] * b_A->size[1] - 1;
5310 for (cartesian_trajectory_planner_B.kend = 0;
5311 cartesian_trajectory_planner_B.kend <=
5312 cartesian_trajectory_planner_B.ix_e;
5313 cartesian_trajectory_planner_B.kend++) {
5314 c_x->data[cartesian_trajectory_planner_B.kend] = b_A->
5315 data[cartesian_trajectory_planner_B.kend];
5316 }
5317
5318 cartesian_trajectory_planner_B.ix_e = cartesian_trajectory_planner_B.pvt *
5319 cartesian_trajectory_planner_B.ma;
5320 for (cartesian_trajectory_planner_B.itemp = 0;
5321 cartesian_trajectory_planner_B.itemp < m;
5322 cartesian_trajectory_planner_B.itemp++) {
5323 cartesian_trajectory_planner_B.scale_f = c_x->
5324 data[cartesian_trajectory_planner_B.ix_e];
5325 c_x->data[cartesian_trajectory_planner_B.ix_e] = c_x->
5326 data[cartesian_trajectory_planner_B.iy_b];
5327 c_x->data[cartesian_trajectory_planner_B.iy_b] =
5328 cartesian_trajectory_planner_B.scale_f;
5329 cartesian_trajectory_planner_B.ix_e++;
5330 cartesian_trajectory_planner_B.iy_b++;
5331 }
5332
5333 cartesian_trajectory_planner_B.kend = b_A->size[0] * b_A->size[1];
5334 b_A->size[0] = c_x->size[0];
5335 b_A->size[1] = c_x->size[1];
5336 cartes_emxEnsureCapacity_real_T(b_A, cartesian_trajectory_planner_B.kend);
5337 cartesian_trajectory_planner_B.ix_e = c_x->size[0] * c_x->size[1] - 1;
5338 for (cartesian_trajectory_planner_B.kend = 0;
5339 cartesian_trajectory_planner_B.kend <=
5340 cartesian_trajectory_planner_B.ix_e;
5341 cartesian_trajectory_planner_B.kend++) {
5342 b_A->data[cartesian_trajectory_planner_B.kend] = c_x->
5343 data[cartesian_trajectory_planner_B.kend];
5344 }
5345
5346 cartesian_trajectory_planner_B.itemp = b_jpvt->
5347 data[cartesian_trajectory_planner_B.pvt];
5348 b_jpvt->data[cartesian_trajectory_planner_B.pvt] = b_jpvt->
5349 data[cartesian_trajectory_planner_B.m_nq];
5350 b_jpvt->data[cartesian_trajectory_planner_B.m_nq] =
5351 cartesian_trajectory_planner_B.itemp;
5352 vn1->data[cartesian_trajectory_planner_B.pvt] = vn1->
5353 data[cartesian_trajectory_planner_B.m_nq];
5354 vn2->data[cartesian_trajectory_planner_B.pvt] = vn2->
5355 data[cartesian_trajectory_planner_B.m_nq];
5356 }
5357
5358 if (cartesian_trajectory_planner_B.m_nq + 1 < m) {
5359 cartesian_trajectory_planner_B.pvt = cartesian_trajectory_planner_B.ii + 2;
5360 cartesian_trajectory_planner_B.kend = c_x->size[0] * c_x->size[1];
5361 c_x->size[0] = b_A->size[0];
5362 c_x->size[1] = b_A->size[1];
5363 cartes_emxEnsureCapacity_real_T(c_x, cartesian_trajectory_planner_B.kend);
5364 cartesian_trajectory_planner_B.ix_e = b_A->size[0] * b_A->size[1] - 1;
5365 for (cartesian_trajectory_planner_B.kend = 0;
5366 cartesian_trajectory_planner_B.kend <=
5367 cartesian_trajectory_planner_B.ix_e;
5368 cartesian_trajectory_planner_B.kend++) {
5369 c_x->data[cartesian_trajectory_planner_B.kend] = b_A->
5370 data[cartesian_trajectory_planner_B.kend];
5371 }
5372
5373 cartesian_trajectory_planner_B.smax = b_A->
5374 data[cartesian_trajectory_planner_B.ii];
5375 tau->data[cartesian_trajectory_planner_B.m_nq] = 0.0;
5376 if (cartesian_trajectory_planner_B.mmi + 1 > 0) {
5377 cartesian_trajectory_planner_B.scale_f = cartesian_trajectory_p_xnrm2_as
5378 (cartesian_trajectory_planner_B.mmi, b_A,
5379 cartesian_trajectory_planner_B.ii + 2);
5380 if (cartesian_trajectory_planner_B.scale_f != 0.0) {
5381 cartesian_trajectory_planner_B.scale_f = rt_hypotd_snf(b_A->
5382 data[cartesian_trajectory_planner_B.ii],
5383 cartesian_trajectory_planner_B.scale_f);
5384 if (b_A->data[cartesian_trajectory_planner_B.ii] >= 0.0) {
5385 cartesian_trajectory_planner_B.scale_f =
5386 -cartesian_trajectory_planner_B.scale_f;
5387 }
5388
5389 if (fabs(cartesian_trajectory_planner_B.scale_f) <
5390 1.0020841800044864E-292) {
5391 cartesian_trajectory_planner_B.kend = -1;
5392 cartesian_trajectory_planner_B.ix_e =
5393 (cartesian_trajectory_planner_B.ii +
5394 cartesian_trajectory_planner_B.mmi) + 1;
5395 do {
5396 cartesian_trajectory_planner_B.kend++;
5397 for (cartesian_trajectory_planner_B.itemp =
5398 cartesian_trajectory_planner_B.pvt;
5399 cartesian_trajectory_planner_B.itemp <=
5400 cartesian_trajectory_planner_B.ix_e;
5401 cartesian_trajectory_planner_B.itemp++) {
5402 c_x->data[cartesian_trajectory_planner_B.itemp - 1] *=
5403 9.9792015476736E+291;
5404 }
5405
5406 cartesian_trajectory_planner_B.scale_f *= 9.9792015476736E+291;
5407 cartesian_trajectory_planner_B.smax *= 9.9792015476736E+291;
5408 } while (!(fabs(cartesian_trajectory_planner_B.scale_f) >=
5409 1.0020841800044864E-292));
5410
5411 cartesian_trajectory_planner_B.scale_f = rt_hypotd_snf
5412 (cartesian_trajectory_planner_B.smax,
5413 cartesian_trajectory_p_xnrm2_as
5414 (cartesian_trajectory_planner_B.mmi, c_x,
5415 cartesian_trajectory_planner_B.ii + 2));
5416 if (cartesian_trajectory_planner_B.smax >= 0.0) {
5417 cartesian_trajectory_planner_B.scale_f =
5418 -cartesian_trajectory_planner_B.scale_f;
5419 }
5420
5421 tau->data[cartesian_trajectory_planner_B.m_nq] =
5422 (cartesian_trajectory_planner_B.scale_f -
5423 cartesian_trajectory_planner_B.smax) /
5424 cartesian_trajectory_planner_B.scale_f;
5425 cartesian_trajectory_planner_B.smax = 1.0 /
5426 (cartesian_trajectory_planner_B.smax -
5427 cartesian_trajectory_planner_B.scale_f);
5428 for (cartesian_trajectory_planner_B.itemp =
5429 cartesian_trajectory_planner_B.pvt;
5430 cartesian_trajectory_planner_B.itemp <=
5431 cartesian_trajectory_planner_B.ix_e;
5432 cartesian_trajectory_planner_B.itemp++) {
5433 c_x->data[cartesian_trajectory_planner_B.itemp - 1] *=
5434 cartesian_trajectory_planner_B.smax;
5435 }
5436
5437 for (cartesian_trajectory_planner_B.itemp = 0;
5438 cartesian_trajectory_planner_B.itemp <=
5439 cartesian_trajectory_planner_B.kend;
5440 cartesian_trajectory_planner_B.itemp++) {
5441 cartesian_trajectory_planner_B.scale_f *= 1.0020841800044864E-292;
5442 }
5443
5444 cartesian_trajectory_planner_B.smax =
5445 cartesian_trajectory_planner_B.scale_f;
5446 } else {
5447 tau->data[cartesian_trajectory_planner_B.m_nq] =
5448 (cartesian_trajectory_planner_B.scale_f - b_A->
5449 data[cartesian_trajectory_planner_B.ii]) /
5450 cartesian_trajectory_planner_B.scale_f;
5451 cartesian_trajectory_planner_B.smax = 1.0 / (b_A->
5452 data[cartesian_trajectory_planner_B.ii] -
5453 cartesian_trajectory_planner_B.scale_f);
5454 cartesian_trajectory_planner_B.kend = c_x->size[0] * c_x->size[1];
5455 c_x->size[0] = b_A->size[0];
5456 c_x->size[1] = b_A->size[1];
5457 cartes_emxEnsureCapacity_real_T(c_x,
5458 cartesian_trajectory_planner_B.kend);
5459 cartesian_trajectory_planner_B.ix_e = b_A->size[0] * b_A->size[1] -
5460 1;
5461 for (cartesian_trajectory_planner_B.kend = 0;
5462 cartesian_trajectory_planner_B.kend <=
5463 cartesian_trajectory_planner_B.ix_e;
5464 cartesian_trajectory_planner_B.kend++) {
5465 c_x->data[cartesian_trajectory_planner_B.kend] = b_A->
5466 data[cartesian_trajectory_planner_B.kend];
5467 }
5468
5469 cartesian_trajectory_planner_B.b_i2 =
5470 (cartesian_trajectory_planner_B.ii +
5471 cartesian_trajectory_planner_B.mmi) + 1;
5472 for (cartesian_trajectory_planner_B.itemp =
5473 cartesian_trajectory_planner_B.pvt;
5474 cartesian_trajectory_planner_B.itemp <=
5475 cartesian_trajectory_planner_B.b_i2;
5476 cartesian_trajectory_planner_B.itemp++) {
5477 c_x->data[cartesian_trajectory_planner_B.itemp - 1] *=
5478 cartesian_trajectory_planner_B.smax;
5479 }
5480
5481 cartesian_trajectory_planner_B.smax =
5482 cartesian_trajectory_planner_B.scale_f;
5483 }
5484 }
5485 }
5486
5487 cartesian_trajectory_planner_B.kend = b_A->size[0] * b_A->size[1];
5488 b_A->size[0] = c_x->size[0];
5489 b_A->size[1] = c_x->size[1];
5490 cartes_emxEnsureCapacity_real_T(b_A, cartesian_trajectory_planner_B.kend);
5491 cartesian_trajectory_planner_B.ix_e = c_x->size[0] * c_x->size[1] - 1;
5492 for (cartesian_trajectory_planner_B.kend = 0;
5493 cartesian_trajectory_planner_B.kend <=
5494 cartesian_trajectory_planner_B.ix_e;
5495 cartesian_trajectory_planner_B.kend++) {
5496 b_A->data[cartesian_trajectory_planner_B.kend] = c_x->
5497 data[cartesian_trajectory_planner_B.kend];
5498 }
5499
5500 b_A->data[cartesian_trajectory_planner_B.ii] =
5501 cartesian_trajectory_planner_B.smax;
5502 } else {
5503 tau->data[cartesian_trajectory_planner_B.m_nq] = 0.0;
5504 }
5505
5506 if (cartesian_trajectory_planner_B.m_nq + 1 < n) {
5507 cartesian_trajectory_planner_B.smax = b_A->
5508 data[cartesian_trajectory_planner_B.ii];
5509 b_A->data[cartesian_trajectory_planner_B.ii] = 1.0;
5510 cartesian_trajectory_planner_B.pvt = (cartesian_trajectory_planner_B.ii +
5511 cartesian_trajectory_planner_B.ma) + 1;
5512 cartesian_trajectory_planner_B.kend = c_x->size[0] * c_x->size[1];
5513 c_x->size[0] = b_A->size[0];
5514 c_x->size[1] = b_A->size[1];
5515 cartes_emxEnsureCapacity_real_T(c_x, cartesian_trajectory_planner_B.kend);
5516 cartesian_trajectory_planner_B.ix_e = b_A->size[0] * b_A->size[1] - 1;
5517 for (cartesian_trajectory_planner_B.kend = 0;
5518 cartesian_trajectory_planner_B.kend <=
5519 cartesian_trajectory_planner_B.ix_e;
5520 cartesian_trajectory_planner_B.kend++) {
5521 c_x->data[cartesian_trajectory_planner_B.kend] = b_A->
5522 data[cartesian_trajectory_planner_B.kend];
5523 }
5524
5525 if (tau->data[cartesian_trajectory_planner_B.m_nq] != 0.0) {
5526 cartesian_trajectory_planner_B.itemp =
5527 cartesian_trajectory_planner_B.mmi;
5528 cartesian_trajectory_planner_B.kend = cartesian_trajectory_planner_B.ii
5529 + cartesian_trajectory_planner_B.mmi;
5530 while ((cartesian_trajectory_planner_B.itemp + 1 > 0) && (b_A->
5531 data[cartesian_trajectory_planner_B.kend] == 0.0)) {
5532 cartesian_trajectory_planner_B.itemp--;
5533 cartesian_trajectory_planner_B.kend--;
5534 }
5535
5536 cartesian_trajectory_planner_B.nmi--;
5537 exitg2 = false;
5538 while ((!exitg2) && (cartesian_trajectory_planner_B.nmi > 0)) {
5539 cartesian_trajectory_planner_B.ix_e =
5540 (cartesian_trajectory_planner_B.nmi - 1) *
5541 cartesian_trajectory_planner_B.ma +
5542 cartesian_trajectory_planner_B.pvt;
5543 cartesian_trajectory_planner_B.kend =
5544 cartesian_trajectory_planner_B.ix_e;
5545 do {
5546 exitg1 = 0;
5547 if (cartesian_trajectory_planner_B.kend <=
5548 cartesian_trajectory_planner_B.ix_e +
5549 cartesian_trajectory_planner_B.itemp) {
5550 if (b_A->data[cartesian_trajectory_planner_B.kend - 1] != 0.0) {
5551 exitg1 = 1;
5552 } else {
5553 cartesian_trajectory_planner_B.kend++;
5554 }
5555 } else {
5556 cartesian_trajectory_planner_B.nmi--;
5557 exitg1 = 2;
5558 }
5559 } while (exitg1 == 0);
5560
5561 if (exitg1 == 1) {
5562 exitg2 = true;
5563 }
5564 }
5565
5566 cartesian_trajectory_planner_B.lastc =
5567 cartesian_trajectory_planner_B.nmi - 1;
5568 cartesian_trajectory_planner_B.kend = c_x->size[0] * c_x->size[1];
5569 c_x->size[0] = b_A->size[0];
5570 c_x->size[1] = b_A->size[1];
5571 cartes_emxEnsureCapacity_real_T(c_x, cartesian_trajectory_planner_B.kend);
5572 cartesian_trajectory_planner_B.ix_e = b_A->size[0] * b_A->size[1] - 1;
5573 for (cartesian_trajectory_planner_B.kend = 0;
5574 cartesian_trajectory_planner_B.kend <=
5575 cartesian_trajectory_planner_B.ix_e;
5576 cartesian_trajectory_planner_B.kend++) {
5577 c_x->data[cartesian_trajectory_planner_B.kend] = b_A->
5578 data[cartesian_trajectory_planner_B.kend];
5579 }
5580 } else {
5581 cartesian_trajectory_planner_B.itemp = -1;
5582 cartesian_trajectory_planner_B.lastc = -1;
5583 }
5584
5585 if (cartesian_trajectory_planner_B.itemp + 1 > 0) {
5586 if (cartesian_trajectory_planner_B.lastc + 1 != 0) {
5587 for (cartesian_trajectory_planner_B.kend = 0;
5588 cartesian_trajectory_planner_B.kend <=
5589 cartesian_trajectory_planner_B.lastc;
5590 cartesian_trajectory_planner_B.kend++) {
5591 work->data[cartesian_trajectory_planner_B.kend] = 0.0;
5592 }
5593
5594 cartesian_trajectory_planner_B.iy_b = 0;
5595 cartesian_trajectory_planner_B.b_i2 =
5596 cartesian_trajectory_planner_B.ma *
5597 cartesian_trajectory_planner_B.lastc +
5598 cartesian_trajectory_planner_B.pvt;
5599 for (cartesian_trajectory_planner_B.nmi =
5600 cartesian_trajectory_planner_B.pvt;
5601 cartesian_trajectory_planner_B.ma < 0 ?
5602 cartesian_trajectory_planner_B.nmi >=
5603 cartesian_trajectory_planner_B.b_i2 :
5604 cartesian_trajectory_planner_B.nmi <=
5605 cartesian_trajectory_planner_B.b_i2;
5606 cartesian_trajectory_planner_B.nmi +=
5607 cartesian_trajectory_planner_B.ma) {
5608 cartesian_trajectory_planner_B.ix_e =
5609 cartesian_trajectory_planner_B.ii;
5610 cartesian_trajectory_planner_B.scale_f = 0.0;
5611 cartesian_trajectory_planner_B.d_a =
5612 cartesian_trajectory_planner_B.nmi +
5613 cartesian_trajectory_planner_B.itemp;
5614 for (cartesian_trajectory_planner_B.kend =
5615 cartesian_trajectory_planner_B.nmi;
5616 cartesian_trajectory_planner_B.kend <=
5617 cartesian_trajectory_planner_B.d_a;
5618 cartesian_trajectory_planner_B.kend++) {
5619 cartesian_trajectory_planner_B.scale_f += c_x->
5620 data[cartesian_trajectory_planner_B.kend - 1] * c_x->
5621 data[cartesian_trajectory_planner_B.ix_e];
5622 cartesian_trajectory_planner_B.ix_e++;
5623 }
5624
5625 work->data[cartesian_trajectory_planner_B.iy_b] +=
5626 cartesian_trajectory_planner_B.scale_f;
5627 cartesian_trajectory_planner_B.iy_b++;
5628 }
5629 }
5630
5631 if (!(-tau->data[cartesian_trajectory_planner_B.m_nq] == 0.0)) {
5632 cartesian_trajectory_planner_B.iy_b = 0;
5633 for (cartesian_trajectory_planner_B.kend = 0;
5634 cartesian_trajectory_planner_B.kend <=
5635 cartesian_trajectory_planner_B.lastc;
5636 cartesian_trajectory_planner_B.kend++) {
5637 if (work->data[cartesian_trajectory_planner_B.iy_b] != 0.0) {
5638 cartesian_trajectory_planner_B.scale_f = work->
5639 data[cartesian_trajectory_planner_B.iy_b] * -tau->
5640 data[cartesian_trajectory_planner_B.m_nq];
5641 cartesian_trajectory_planner_B.ix_e =
5642 cartesian_trajectory_planner_B.ii;
5643 cartesian_trajectory_planner_B.b_i2 =
5644 cartesian_trajectory_planner_B.itemp +
5645 cartesian_trajectory_planner_B.pvt;
5646 for (cartesian_trajectory_planner_B.nmi =
5647 cartesian_trajectory_planner_B.pvt;
5648 cartesian_trajectory_planner_B.nmi <=
5649 cartesian_trajectory_planner_B.b_i2;
5650 cartesian_trajectory_planner_B.nmi++) {
5651 c_x->data[cartesian_trajectory_planner_B.nmi - 1] += c_x->
5652 data[cartesian_trajectory_planner_B.ix_e] *
5653 cartesian_trajectory_planner_B.scale_f;
5654 cartesian_trajectory_planner_B.ix_e++;
5655 }
5656 }
5657
5658 cartesian_trajectory_planner_B.iy_b++;
5659 cartesian_trajectory_planner_B.pvt +=
5660 cartesian_trajectory_planner_B.ma;
5661 }
5662 }
5663 }
5664
5665 cartesian_trajectory_planner_B.kend = b_A->size[0] * b_A->size[1];
5666 b_A->size[0] = c_x->size[0];
5667 b_A->size[1] = c_x->size[1];
5668 cartes_emxEnsureCapacity_real_T(b_A, cartesian_trajectory_planner_B.kend);
5669 cartesian_trajectory_planner_B.ix_e = c_x->size[0] * c_x->size[1] - 1;
5670 for (cartesian_trajectory_planner_B.kend = 0;
5671 cartesian_trajectory_planner_B.kend <=
5672 cartesian_trajectory_planner_B.ix_e;
5673 cartesian_trajectory_planner_B.kend++) {
5674 b_A->data[cartesian_trajectory_planner_B.kend] = c_x->
5675 data[cartesian_trajectory_planner_B.kend];
5676 }
5677
5678 b_A->data[cartesian_trajectory_planner_B.ii] =
5679 cartesian_trajectory_planner_B.smax;
5680 }
5681
5682 for (cartesian_trajectory_planner_B.ii = cartesian_trajectory_planner_B.m_nq
5683 + 2; cartesian_trajectory_planner_B.ii <= n;
5684 cartesian_trajectory_planner_B.ii++) {
5685 cartesian_trajectory_planner_B.pvt = ((cartesian_trajectory_planner_B.ii -
5686 1) * cartesian_trajectory_planner_B.ma +
5687 cartesian_trajectory_planner_B.m_nq) + 1;
5688 if (vn1->data[cartesian_trajectory_planner_B.ii - 1] != 0.0) {
5689 cartesian_trajectory_planner_B.smax = fabs(b_A->
5690 data[cartesian_trajectory_planner_B.pvt - 1]) / vn1->
5691 data[cartesian_trajectory_planner_B.ii - 1];
5692 cartesian_trajectory_planner_B.smax = 1.0 -
5693 cartesian_trajectory_planner_B.smax *
5694 cartesian_trajectory_planner_B.smax;
5695 if (cartesian_trajectory_planner_B.smax < 0.0) {
5696 cartesian_trajectory_planner_B.smax = 0.0;
5697 }
5698
5699 cartesian_trajectory_planner_B.scale_f = vn1->
5700 data[cartesian_trajectory_planner_B.ii - 1] / vn2->
5701 data[cartesian_trajectory_planner_B.ii - 1];
5702 cartesian_trajectory_planner_B.scale_f =
5703 cartesian_trajectory_planner_B.scale_f *
5704 cartesian_trajectory_planner_B.scale_f *
5705 cartesian_trajectory_planner_B.smax;
5706 if (cartesian_trajectory_planner_B.scale_f <= 1.4901161193847656E-8) {
5707 if (cartesian_trajectory_planner_B.m_nq + 1 < m) {
5708 cartesian_trajectory_planner_B.smax = 0.0;
5709 if (cartesian_trajectory_planner_B.mmi >= 1) {
5710 if (cartesian_trajectory_planner_B.mmi == 1) {
5711 cartesian_trajectory_planner_B.smax = fabs(b_A->
5712 data[cartesian_trajectory_planner_B.pvt]);
5713 } else {
5714 cartesian_trajectory_planner_B.scale_f = 3.3121686421112381E-170;
5715 cartesian_trajectory_planner_B.kend =
5716 cartesian_trajectory_planner_B.pvt +
5717 cartesian_trajectory_planner_B.mmi;
5718 for (cartesian_trajectory_planner_B.itemp =
5719 cartesian_trajectory_planner_B.pvt + 1;
5720 cartesian_trajectory_planner_B.itemp <=
5721 cartesian_trajectory_planner_B.kend;
5722 cartesian_trajectory_planner_B.itemp++) {
5723 cartesian_trajectory_planner_B.absxk = fabs(b_A->
5724 data[cartesian_trajectory_planner_B.itemp - 1]);
5725 if (cartesian_trajectory_planner_B.absxk >
5726 cartesian_trajectory_planner_B.scale_f) {
5727 cartesian_trajectory_planner_B.t =
5728 cartesian_trajectory_planner_B.scale_f /
5729 cartesian_trajectory_planner_B.absxk;
5730 cartesian_trajectory_planner_B.smax =
5731 cartesian_trajectory_planner_B.smax *
5732 cartesian_trajectory_planner_B.t *
5733 cartesian_trajectory_planner_B.t + 1.0;
5734 cartesian_trajectory_planner_B.scale_f =
5735 cartesian_trajectory_planner_B.absxk;
5736 } else {
5737 cartesian_trajectory_planner_B.t =
5738 cartesian_trajectory_planner_B.absxk /
5739 cartesian_trajectory_planner_B.scale_f;
5740 cartesian_trajectory_planner_B.smax +=
5741 cartesian_trajectory_planner_B.t *
5742 cartesian_trajectory_planner_B.t;
5743 }
5744 }
5745
5746 cartesian_trajectory_planner_B.smax =
5747 cartesian_trajectory_planner_B.scale_f * sqrt
5748 (cartesian_trajectory_planner_B.smax);
5749 }
5750 }
5751
5752 vn1->data[cartesian_trajectory_planner_B.ii - 1] =
5753 cartesian_trajectory_planner_B.smax;
5754 vn2->data[cartesian_trajectory_planner_B.ii - 1] = vn1->
5755 data[cartesian_trajectory_planner_B.ii - 1];
5756 } else {
5757 vn1->data[cartesian_trajectory_planner_B.ii - 1] = 0.0;
5758 vn2->data[cartesian_trajectory_planner_B.ii - 1] = 0.0;
5759 }
5760 } else {
5761 vn1->data[cartesian_trajectory_planner_B.ii - 1] *= sqrt
5762 (cartesian_trajectory_planner_B.smax);
5763 }
5764 }
5765 }
5766 }
5767
5768 cartesian_trajec_emxFree_real_T(&c_x);
5769 cartesian_trajec_emxFree_real_T(&vn2);
5770 cartesian_trajec_emxFree_real_T(&vn1);
5771 cartesian_trajec_emxFree_real_T(&work);
5772}
5773
5774static void cartesian_trajectory_pl_xzgetrf(int32_T m, int32_T n, const
5775 emxArray_real_T_cartesian_tra_T *A, int32_T lda,
5776 emxArray_real_T_cartesian_tra_T *b_A, emxArray_int32_T_cartesian_tr_T *ipiv,
5777 int32_T *info)
5778{
5779 int32_T mmj;
5780 int32_T b;
5781 int32_T c;
5782 emxArray_real_T_cartesian_tra_T *c_x;
5783 int32_T ix;
5784 real_T smax;
5785 real_T s;
5786 int32_T iy;
5787 int32_T n_0;
5788 int32_T yk;
5789 int32_T k;
5790 int32_T jA;
5791 int32_T jy;
5792 int32_T c_0;
5793 int32_T c_tmp;
5794 k = b_A->size[0] * b_A->size[1];
5795 b_A->size[0] = A->size[0];
5796 b_A->size[1] = A->size[1];
5797 cartes_emxEnsureCapacity_real_T(b_A, k);
5798 iy = A->size[0] * A->size[1] - 1;
5799 for (k = 0; k <= iy; k++) {
5800 b_A->data[k] = A->data[k];
5801 }
5802
5803 if (m < n) {
5804 n_0 = m;
5805 } else {
5806 n_0 = n;
5807 }
5808
5809 if (n_0 < 1) {
5810 n_0 = 0;
5811 }
5812
5813 k = ipiv->size[0] * ipiv->size[1];
5814 ipiv->size[0] = 1;
5815 ipiv->size[1] = n_0;
5816 carte_emxEnsureCapacity_int32_T(ipiv, k);
5817 if (n_0 > 0) {
5818 ipiv->data[0] = 1;
5819 yk = 1;
5820 for (k = 2; k <= n_0; k++) {
5821 yk++;
5822 ipiv->data[k - 1] = yk;
5823 }
5824 }
5825
5826 yk = 0;
5827 cartesian_trajec_emxInit_real_T(&c_x, 2);
5828 if ((m < 1) || (n < 1)) {
5829 } else {
5830 n_0 = m - 1;
5831 if (n_0 >= n) {
5832 n_0 = n;
5833 }
5834
5835 b = n_0 - 1;
5836 for (n_0 = 0; n_0 <= b; n_0++) {
5837 mmj = m - n_0;
5838 c_tmp = (lda + 1) * n_0;
5839 c = c_tmp + 2;
5840 if (mmj < 1) {
5841 iy = 0;
5842 } else {
5843 iy = 1;
5844 if (mmj > 1) {
5845 ix = c - 2;
5846 smax = fabs(b_A->data[c_tmp]);
5847 for (k = 2; k <= mmj; k++) {
5848 ix++;
5849 s = fabs(b_A->data[ix]);
5850 if (s > smax) {
5851 iy = k;
5852 smax = s;
5853 }
5854 }
5855 }
5856 }
5857
5858 if (b_A->data[(c + iy) - 3] != 0.0) {
5859 if (iy - 1 != 0) {
5860 k = n_0 + iy;
5861 ipiv->data[n_0] = k;
5862 ix = c_x->size[0] * c_x->size[1];
5863 c_x->size[0] = b_A->size[0];
5864 c_x->size[1] = b_A->size[1];
5865 cartes_emxEnsureCapacity_real_T(c_x, ix);
5866 iy = b_A->size[0] * b_A->size[1] - 1;
5867 for (ix = 0; ix <= iy; ix++) {
5868 c_x->data[ix] = b_A->data[ix];
5869 }
5870
5871 ix = n_0;
5872 iy = k - 1;
5873 for (k = 0; k < n; k++) {
5874 smax = c_x->data[ix];
5875 c_x->data[ix] = c_x->data[iy];
5876 c_x->data[iy] = smax;
5877 ix += lda;
5878 iy += lda;
5879 }
5880
5881 k = b_A->size[0] * b_A->size[1];
5882 b_A->size[0] = c_x->size[0];
5883 b_A->size[1] = c_x->size[1];
5884 cartes_emxEnsureCapacity_real_T(b_A, k);
5885 iy = c_x->size[0] * c_x->size[1] - 1;
5886 for (k = 0; k <= iy; k++) {
5887 b_A->data[k] = c_x->data[k];
5888 }
5889 }
5890
5891 iy = c + mmj;
5892 for (k = c; k <= iy - 2; k++) {
5893 b_A->data[k - 1] /= b_A->data[c_tmp];
5894 }
5895 } else {
5896 yk = n_0 + 1;
5897 }
5898
5899 iy = (n - n_0) - 2;
5900 jy = c_tmp + lda;
5901 jA = jy + 1;
5902 for (k = 0; k <= iy; k++) {
5903 smax = b_A->data[jy];
5904 if (b_A->data[jy] != 0.0) {
5905 ix = c - 1;
5906 c_0 = (mmj + jA) - 1;
5907 for (c_tmp = jA + 1; c_tmp <= c_0; c_tmp++) {
5908 b_A->data[c_tmp - 1] += b_A->data[ix] * -smax;
5909 ix++;
5910 }
5911 }
5912
5913 jy += lda;
5914 jA += lda;
5915 }
5916 }
5917
5918 if ((yk == 0) && (m <= n) && (!(b_A->data[((m - 1) * b_A->size[0] + m) - 1]
5919 != 0.0))) {
5920 yk = m;
5921 }
5922 }
5923
5924 cartesian_trajec_emxFree_real_T(&c_x);
5925 *info = yk;
5926}
5927
5928static void cartesian_trajectory_plan_xtrsm(int32_T m, int32_T n, const
5929 emxArray_real_T_cartesian_tra_T *A, int32_T lda, const
5930 emxArray_real_T_cartesian_tra_T *B, int32_T ldb,
5931 emxArray_real_T_cartesian_tra_T *b_B)
5932{
5933 int32_T jBcol;
5934 int32_T kAcol;
5935 int32_T i;
5936 int32_T k;
5937 int32_T b;
5938 int32_T b_i;
5939 int32_T loop_ub;
5940 int32_T i_0;
5941 int32_T tmp;
5942 i_0 = b_B->size[0] * b_B->size[1];
5943 b_B->size[0] = B->size[0];
5944 b_B->size[1] = B->size[1];
5945 cartes_emxEnsureCapacity_real_T(b_B, i_0);
5946 loop_ub = B->size[0] * B->size[1] - 1;
5947 for (i_0 = 0; i_0 <= loop_ub; i_0++) {
5948 b_B->data[i_0] = B->data[i_0];
5949 }
5950
5951 if ((n == 0) || ((B->size[0] == 0) || (B->size[1] == 0))) {
5952 } else {
5953 for (loop_ub = 0; loop_ub < n; loop_ub++) {
5954 jBcol = ldb * loop_ub - 1;
5955 for (k = m; k >= 1; k--) {
5956 kAcol = (k - 1) * lda - 1;
5957 i_0 = k + jBcol;
5958 if (b_B->data[i_0] != 0.0) {
5959 b_B->data[i_0] /= A->data[k + kAcol];
5960 b = k - 2;
5961 for (b_i = 0; b_i <= b; b_i++) {
5962 i = b_i + 1;
5963 tmp = i + jBcol;
5964 b_B->data[tmp] -= b_B->data[i_0] * A->data[i + kAcol];
5965 }
5966 }
5967 }
5968 }
5969 }
5970}
5971
5972static void cartesian_traje_emxFree_int32_T(emxArray_int32_T_cartesian_tr_T
5973 **pEmxArray)
5974{
5975 if (*pEmxArray != (emxArray_int32_T_cartesian_tr_T *)NULL) {
5976 if (((*pEmxArray)->data != (int32_T *)NULL) && (*pEmxArray)->canFreeData) {
5977 free((*pEmxArray)->data);
5978 }
5979
5980 free((*pEmxArray)->size);
5981 free(*pEmxArray);
5982 *pEmxArray = (emxArray_int32_T_cartesian_tr_T *)NULL;
5983 }
5984}
5985
5986static void cartesian_trajectory_p_mldivide(const
5987 emxArray_real_T_cartesian_tra_T *A, const emxArray_real_T_cartesian_tra_T *B,
5988 emxArray_real_T_cartesian_tra_T *Y)
5989{
5990 emxArray_real_T_cartesian_tra_T *c_A;
5991 emxArray_real_T_cartesian_tra_T *b_tau;
5992 emxArray_int32_T_cartesian_tr_T *b_jpvt;
5993 emxArray_real_T_cartesian_tra_T *B_0;
5994 emxArray_int32_T_cartesian_tr_T *b_jpvt_0;
5995 boolean_T guard1 = false;
5996 cartesian_trajec_emxInit_real_T(&c_A, 2);
5997 cartesian_trajec_emxInit_real_T(&b_tau, 1);
5998 cartesian_traje_emxInit_int32_T(&b_jpvt, 2);
5999 cartesian_trajec_emxInit_real_T(&B_0, 2);
6000 cartesian_traje_emxInit_int32_T(&b_jpvt_0, 2);
6001 if ((A->size[0] == 0) || (A->size[1] == 0) || ((B->size[0] == 0) || (B->size[1]
6002 == 0))) {
6003 cartesian_trajectory_planner_B.minmn = A->size[1];
6004 cartesian_trajectory_planner_B.minmana = B->size[1];
6005 cartesian_trajectory_planner_B.b_i_fn = Y->size[0] * Y->size[1];
6006 Y->size[0] = cartesian_trajectory_planner_B.minmn;
6007 Y->size[1] = cartesian_trajectory_planner_B.minmana;
6008 cartes_emxEnsureCapacity_real_T(Y, cartesian_trajectory_planner_B.b_i_fn);
6009 cartesian_trajectory_planner_B.minmn = cartesian_trajectory_planner_B.minmn *
6010 cartesian_trajectory_planner_B.minmana - 1;
6011 for (cartesian_trajectory_planner_B.b_i_fn = 0;
6012 cartesian_trajectory_planner_B.b_i_fn <=
6013 cartesian_trajectory_planner_B.minmn;
6014 cartesian_trajectory_planner_B.b_i_fn++) {
6015 Y->data[cartesian_trajectory_planner_B.b_i_fn] = 0.0;
6016 }
6017 } else if (A->size[0] == A->size[1]) {
6018 cartesian_trajectory_planner_B.minmn = A->size[0];
6019 cartesian_trajectory_planner_B.rankR = A->size[1];
6020 if (cartesian_trajectory_planner_B.minmn <
6021 cartesian_trajectory_planner_B.rankR) {
6022 cartesian_trajectory_planner_B.rankR =
6023 cartesian_trajectory_planner_B.minmn;
6024 }
6025
6026 cartesian_trajectory_planner_B.minmn = B->size[0];
6027 if (cartesian_trajectory_planner_B.minmn <
6028 cartesian_trajectory_planner_B.rankR) {
6029 cartesian_trajectory_planner_B.rankR =
6030 cartesian_trajectory_planner_B.minmn;
6031 }
6032
6033 cartesian_trajectory_planner_B.nb = B->size[1] - 1;
6034 cartesian_trajectory_pl_xzgetrf(cartesian_trajectory_planner_B.rankR,
6035 cartesian_trajectory_planner_B.rankR, A, A->size[0], c_A, b_jpvt,
6036 &cartesian_trajectory_planner_B.minmn);
6037 cartesian_trajectory_planner_B.b_i_fn = B_0->size[0] * B_0->size[1];
6038 B_0->size[0] = B->size[0];
6039 B_0->size[1] = B->size[1];
6040 cartes_emxEnsureCapacity_real_T(B_0, cartesian_trajectory_planner_B.b_i_fn);
6041 cartesian_trajectory_planner_B.minmn = B->size[0] * B->size[1] - 1;
6042 for (cartesian_trajectory_planner_B.b_i_fn = 0;
6043 cartesian_trajectory_planner_B.b_i_fn <=
6044 cartesian_trajectory_planner_B.minmn;
6045 cartesian_trajectory_planner_B.b_i_fn++) {
6046 B_0->data[cartesian_trajectory_planner_B.b_i_fn] = B->
6047 data[cartesian_trajectory_planner_B.b_i_fn];
6048 }
6049
6050 cartesian_trajectory_planner_B.minmn = cartesian_trajectory_planner_B.rankR
6051 - 2;
6052 for (cartesian_trajectory_planner_B.b_i_fn = 0;
6053 cartesian_trajectory_planner_B.b_i_fn <=
6054 cartesian_trajectory_planner_B.minmn;
6055 cartesian_trajectory_planner_B.b_i_fn++) {
6056 if (cartesian_trajectory_planner_B.b_i_fn + 1 != b_jpvt->
6057 data[cartesian_trajectory_planner_B.b_i_fn]) {
6058 cartesian_trajectory_planner_B.na = b_jpvt->
6059 data[cartesian_trajectory_planner_B.b_i_fn] - 1;
6060 for (cartesian_trajectory_planner_B.minmana = 0;
6061 cartesian_trajectory_planner_B.minmana <=
6062 cartesian_trajectory_planner_B.nb;
6063 cartesian_trajectory_planner_B.minmana++) {
6064 cartesian_trajectory_planner_B.tol_i = B_0->data[B_0->size[0] *
6065 cartesian_trajectory_planner_B.minmana +
6066 cartesian_trajectory_planner_B.b_i_fn];
6067 B_0->data[cartesian_trajectory_planner_B.b_i_fn + B_0->size[0] *
6068 cartesian_trajectory_planner_B.minmana] = B_0->data[B_0->size[0] *
6069 cartesian_trajectory_planner_B.minmana +
6070 cartesian_trajectory_planner_B.na];
6071 B_0->data[cartesian_trajectory_planner_B.na + B_0->size[0] *
6072 cartesian_trajectory_planner_B.minmana] =
6073 cartesian_trajectory_planner_B.tol_i;
6074 }
6075 }
6076 }
6077
6078 if ((B->size[1] == 0) || ((B_0->size[0] == 0) || (B_0->size[1] == 0))) {
6079 } else {
6080 for (cartesian_trajectory_planner_B.minmana = 0;
6081 cartesian_trajectory_planner_B.minmana <=
6082 cartesian_trajectory_planner_B.nb;
6083 cartesian_trajectory_planner_B.minmana++) {
6084 cartesian_trajectory_planner_B.m_e = B->size[0] *
6085 cartesian_trajectory_planner_B.minmana - 1;
6086 for (cartesian_trajectory_planner_B.minmn = 0;
6087 cartesian_trajectory_planner_B.minmn <
6088 cartesian_trajectory_planner_B.rankR;
6089 cartesian_trajectory_planner_B.minmn++) {
6090 cartesian_trajectory_planner_B.nb_n = c_A->size[0] *
6091 cartesian_trajectory_planner_B.minmn - 1;
6092 cartesian_trajectory_planner_B.b_i_fn =
6093 (cartesian_trajectory_planner_B.minmn +
6094 cartesian_trajectory_planner_B.m_e) + 1;
6095 if (B_0->data[cartesian_trajectory_planner_B.b_i_fn] != 0.0) {
6096 for (cartesian_trajectory_planner_B.na =
6097 cartesian_trajectory_planner_B.minmn + 2;
6098 cartesian_trajectory_planner_B.na <=
6099 cartesian_trajectory_planner_B.rankR;
6100 cartesian_trajectory_planner_B.na++) {
6101 cartesian_trajectory_planner_B.mn =
6102 cartesian_trajectory_planner_B.na +
6103 cartesian_trajectory_planner_B.m_e;
6104 B_0->data[cartesian_trajectory_planner_B.mn] -= B_0->
6105 data[cartesian_trajectory_planner_B.b_i_fn] * c_A->
6106 data[cartesian_trajectory_planner_B.na +
6107 cartesian_trajectory_planner_B.nb_n];
6108 }
6109 }
6110 }
6111 }
6112 }
6113
6114 cartesian_trajectory_plan_xtrsm(cartesian_trajectory_planner_B.rankR,
6115 B->size[1], c_A, c_A->size[0], B_0, B->size[0], Y);
6116 } else {
6117 cartesian_trajectory_planner_B.na = A->size[1] - 1;
6118 cartesian_trajectory_planner_B.b_i_fn = c_A->size[0] * c_A->size[1];
6119 c_A->size[0] = A->size[0];
6120 c_A->size[1] = A->size[1];
6121 cartes_emxEnsureCapacity_real_T(c_A, cartesian_trajectory_planner_B.b_i_fn);
6122 cartesian_trajectory_planner_B.minmn = A->size[0] * A->size[1] - 1;
6123 for (cartesian_trajectory_planner_B.b_i_fn = 0;
6124 cartesian_trajectory_planner_B.b_i_fn <=
6125 cartesian_trajectory_planner_B.minmn;
6126 cartesian_trajectory_planner_B.b_i_fn++) {
6127 c_A->data[cartesian_trajectory_planner_B.b_i_fn] = A->
6128 data[cartesian_trajectory_planner_B.b_i_fn];
6129 }
6130
6131 cartesian_trajectory_planner_B.minmn = A->size[0];
6132 cartesian_trajectory_planner_B.minmana = A->size[1];
6133 if (cartesian_trajectory_planner_B.minmn <
6134 cartesian_trajectory_planner_B.minmana) {
6135 cartesian_trajectory_planner_B.minmana =
6136 cartesian_trajectory_planner_B.minmn;
6137 }
6138
6139 cartesian_trajectory_planner_B.b_i_fn = b_tau->size[0];
6140 b_tau->size[0] = cartesian_trajectory_planner_B.minmana;
6141 cartes_emxEnsureCapacity_real_T(b_tau, cartesian_trajectory_planner_B.b_i_fn);
6142 for (cartesian_trajectory_planner_B.b_i_fn = 0;
6143 cartesian_trajectory_planner_B.b_i_fn <
6144 cartesian_trajectory_planner_B.minmana;
6145 cartesian_trajectory_planner_B.b_i_fn++) {
6146 b_tau->data[cartesian_trajectory_planner_B.b_i_fn] = 0.0;
6147 }
6148
6149 guard1 = false;
6150 if ((A->size[0] == 0) || (A->size[1] == 0)) {
6151 guard1 = true;
6152 } else {
6153 cartesian_trajectory_planner_B.minmn = A->size[0];
6154 cartesian_trajectory_planner_B.minmana = A->size[1];
6155 if (cartesian_trajectory_planner_B.minmn <
6156 cartesian_trajectory_planner_B.minmana) {
6157 cartesian_trajectory_planner_B.minmana =
6158 cartesian_trajectory_planner_B.minmn;
6159 }
6160
6161 if (cartesian_trajectory_planner_B.minmana < 1) {
6162 guard1 = true;
6163 } else {
6164 cartesian_trajectory_planner_B.b_i_fn = b_jpvt->size[0] * b_jpvt->size[1];
6165 b_jpvt->size[0] = 1;
6166 b_jpvt->size[1] = A->size[1];
6167 carte_emxEnsureCapacity_int32_T(b_jpvt,
6168 cartesian_trajectory_planner_B.b_i_fn);
6169 cartesian_trajectory_planner_B.minmn = A->size[1] - 1;
6170 for (cartesian_trajectory_planner_B.b_i_fn = 0;
6171 cartesian_trajectory_planner_B.b_i_fn <=
6172 cartesian_trajectory_planner_B.minmn;
6173 cartesian_trajectory_planner_B.b_i_fn++) {
6174 b_jpvt->data[cartesian_trajectory_planner_B.b_i_fn] = 0;
6175 }
6176
6177 for (cartesian_trajectory_planner_B.minmn = 0;
6178 cartesian_trajectory_planner_B.minmn <=
6179 cartesian_trajectory_planner_B.na;
6180 cartesian_trajectory_planner_B.minmn++) {
6181 b_jpvt->data[cartesian_trajectory_planner_B.minmn] =
6182 cartesian_trajectory_planner_B.minmn + 1;
6183 }
6184
6185 cartesian_trajectory_planner_B.b_i_fn = b_jpvt_0->size[0] *
6186 b_jpvt_0->size[1];
6187 b_jpvt_0->size[0] = 1;
6188 b_jpvt_0->size[1] = b_jpvt->size[1];
6189 carte_emxEnsureCapacity_int32_T(b_jpvt_0,
6190 cartesian_trajectory_planner_B.b_i_fn);
6191 cartesian_trajectory_planner_B.minmn = b_jpvt->size[0] * b_jpvt->size[1];
6192 for (cartesian_trajectory_planner_B.b_i_fn = 0;
6193 cartesian_trajectory_planner_B.b_i_fn <
6194 cartesian_trajectory_planner_B.minmn;
6195 cartesian_trajectory_planner_B.b_i_fn++) {
6196 b_jpvt_0->data[cartesian_trajectory_planner_B.b_i_fn] = b_jpvt->
6197 data[cartesian_trajectory_planner_B.b_i_fn];
6198 }
6199
6200 cartesian_trajectory_pla_qrpf_a(A, A->size[0], A->size[1], b_tau,
6201 b_jpvt_0, c_A, b_jpvt);
6202 }
6203 }
6204
6205 if (guard1) {
6206 cartesian_trajectory_planner_B.b_i_fn = b_jpvt->size[0] * b_jpvt->size[1];
6207 b_jpvt->size[0] = 1;
6208 b_jpvt->size[1] = A->size[1];
6209 carte_emxEnsureCapacity_int32_T(b_jpvt,
6210 cartesian_trajectory_planner_B.b_i_fn);
6211 cartesian_trajectory_planner_B.minmn = A->size[1] - 1;
6212 for (cartesian_trajectory_planner_B.b_i_fn = 0;
6213 cartesian_trajectory_planner_B.b_i_fn <=
6214 cartesian_trajectory_planner_B.minmn;
6215 cartesian_trajectory_planner_B.b_i_fn++) {
6216 b_jpvt->data[cartesian_trajectory_planner_B.b_i_fn] = 0;
6217 }
6218
6219 for (cartesian_trajectory_planner_B.minmana = 0;
6220 cartesian_trajectory_planner_B.minmana <=
6221 cartesian_trajectory_planner_B.na;
6222 cartesian_trajectory_planner_B.minmana++) {
6223 b_jpvt->data[cartesian_trajectory_planner_B.minmana] =
6224 cartesian_trajectory_planner_B.minmana + 1;
6225 }
6226 }
6227
6228 cartesian_trajectory_planner_B.rankR = 0;
6229 if (c_A->size[0] < c_A->size[1]) {
6230 cartesian_trajectory_planner_B.minmn = c_A->size[0];
6231 cartesian_trajectory_planner_B.minmana = c_A->size[1];
6232 } else {
6233 cartesian_trajectory_planner_B.minmn = c_A->size[1];
6234 cartesian_trajectory_planner_B.minmana = c_A->size[0];
6235 }
6236
6237 if (cartesian_trajectory_planner_B.minmn > 0) {
6238 cartesian_trajectory_planner_B.tol_i = 2.2204460492503131E-15 *
6239 static_cast<real_T>(cartesian_trajectory_planner_B.minmana);
6240 if (1.4901161193847656E-8 < cartesian_trajectory_planner_B.tol_i) {
6241 cartesian_trajectory_planner_B.tol_i = 1.4901161193847656E-8;
6242 }
6243
6244 cartesian_trajectory_planner_B.tol_i *= fabs(c_A->data[0]);
6245 while ((cartesian_trajectory_planner_B.rankR <
6246 cartesian_trajectory_planner_B.minmn) && (!(fabs(c_A->data
6247 [c_A->size[0] * cartesian_trajectory_planner_B.rankR +
6248 cartesian_trajectory_planner_B.rankR]) <=
6249 cartesian_trajectory_planner_B.tol_i))) {
6250 cartesian_trajectory_planner_B.rankR++;
6251 }
6252 }
6253
6254 cartesian_trajectory_planner_B.nb = B->size[1] - 1;
6255 cartesian_trajectory_planner_B.minmn = c_A->size[1];
6256 cartesian_trajectory_planner_B.minmana = B->size[1];
6257 cartesian_trajectory_planner_B.b_i_fn = Y->size[0] * Y->size[1];
6258 Y->size[0] = cartesian_trajectory_planner_B.minmn;
6259 Y->size[1] = cartesian_trajectory_planner_B.minmana;
6260 cartes_emxEnsureCapacity_real_T(Y, cartesian_trajectory_planner_B.b_i_fn);
6261 cartesian_trajectory_planner_B.minmn = cartesian_trajectory_planner_B.minmn *
6262 cartesian_trajectory_planner_B.minmana - 1;
6263 for (cartesian_trajectory_planner_B.b_i_fn = 0;
6264 cartesian_trajectory_planner_B.b_i_fn <=
6265 cartesian_trajectory_planner_B.minmn;
6266 cartesian_trajectory_planner_B.b_i_fn++) {
6267 Y->data[cartesian_trajectory_planner_B.b_i_fn] = 0.0;
6268 }
6269
6270 cartesian_trajectory_planner_B.b_i_fn = B_0->size[0] * B_0->size[1];
6271 B_0->size[0] = B->size[0];
6272 B_0->size[1] = B->size[1];
6273 cartes_emxEnsureCapacity_real_T(B_0, cartesian_trajectory_planner_B.b_i_fn);
6274 cartesian_trajectory_planner_B.minmn = B->size[0] * B->size[1] - 1;
6275 for (cartesian_trajectory_planner_B.b_i_fn = 0;
6276 cartesian_trajectory_planner_B.b_i_fn <=
6277 cartesian_trajectory_planner_B.minmn;
6278 cartesian_trajectory_planner_B.b_i_fn++) {
6279 B_0->data[cartesian_trajectory_planner_B.b_i_fn] = B->
6280 data[cartesian_trajectory_planner_B.b_i_fn];
6281 }
6282
6283 cartesian_trajectory_planner_B.m_e = c_A->size[0];
6284 cartesian_trajectory_planner_B.nb_n = B->size[1] - 1;
6285 cartesian_trajectory_planner_B.minmn = c_A->size[0];
6286 cartesian_trajectory_planner_B.minmana = c_A->size[1];
6287 if (cartesian_trajectory_planner_B.minmn <
6288 cartesian_trajectory_planner_B.minmana) {
6289 cartesian_trajectory_planner_B.minmana =
6290 cartesian_trajectory_planner_B.minmn;
6291 }
6292
6293 cartesian_trajectory_planner_B.mn = cartesian_trajectory_planner_B.minmana -
6294 1;
6295 for (cartesian_trajectory_planner_B.minmana = 0;
6296 cartesian_trajectory_planner_B.minmana <=
6297 cartesian_trajectory_planner_B.mn;
6298 cartesian_trajectory_planner_B.minmana++) {
6299 if (b_tau->data[cartesian_trajectory_planner_B.minmana] != 0.0) {
6300 for (cartesian_trajectory_planner_B.minmn = 0;
6301 cartesian_trajectory_planner_B.minmn <=
6302 cartesian_trajectory_planner_B.nb_n;
6303 cartesian_trajectory_planner_B.minmn++) {
6304 cartesian_trajectory_planner_B.tol_i = B_0->data[B_0->size[0] *
6305 cartesian_trajectory_planner_B.minmn +
6306 cartesian_trajectory_planner_B.minmana];
6307 for (cartesian_trajectory_planner_B.na =
6308 cartesian_trajectory_planner_B.minmana + 2;
6309 cartesian_trajectory_planner_B.na <=
6310 cartesian_trajectory_planner_B.m_e;
6311 cartesian_trajectory_planner_B.na++) {
6312 cartesian_trajectory_planner_B.tol_i += c_A->data[(c_A->size[0] *
6313 cartesian_trajectory_planner_B.minmana +
6314 cartesian_trajectory_planner_B.na) - 1] * B_0->data[(B_0->size[0] *
6315 cartesian_trajectory_planner_B.minmn +
6316 cartesian_trajectory_planner_B.na) - 1];
6317 }
6318
6319 cartesian_trajectory_planner_B.tol_i *= b_tau->
6320 data[cartesian_trajectory_planner_B.minmana];
6321 if (cartesian_trajectory_planner_B.tol_i != 0.0) {
6322 B_0->data[cartesian_trajectory_planner_B.minmana + B_0->size[0] *
6323 cartesian_trajectory_planner_B.minmn] -=
6324 cartesian_trajectory_planner_B.tol_i;
6325 for (cartesian_trajectory_planner_B.b_i_fn =
6326 cartesian_trajectory_planner_B.minmana + 2;
6327 cartesian_trajectory_planner_B.b_i_fn <=
6328 cartesian_trajectory_planner_B.m_e;
6329 cartesian_trajectory_planner_B.b_i_fn++) {
6330 B_0->data[(cartesian_trajectory_planner_B.b_i_fn + B_0->size[0] *
6331 cartesian_trajectory_planner_B.minmn) - 1] -= c_A->
6332 data[(c_A->size[0] * cartesian_trajectory_planner_B.minmana +
6333 cartesian_trajectory_planner_B.b_i_fn) - 1] *
6334 cartesian_trajectory_planner_B.tol_i;
6335 }
6336 }
6337 }
6338 }
6339 }
6340
6341 for (cartesian_trajectory_planner_B.minmn = 0;
6342 cartesian_trajectory_planner_B.minmn <=
6343 cartesian_trajectory_planner_B.nb; cartesian_trajectory_planner_B.minmn
6344 ++) {
6345 for (cartesian_trajectory_planner_B.b_i_fn = 0;
6346 cartesian_trajectory_planner_B.b_i_fn <
6347 cartesian_trajectory_planner_B.rankR;
6348 cartesian_trajectory_planner_B.b_i_fn++) {
6349 Y->data[(b_jpvt->data[cartesian_trajectory_planner_B.b_i_fn] + Y->size[0]
6350 * cartesian_trajectory_planner_B.minmn) - 1] = B_0->data
6351 [B_0->size[0] * cartesian_trajectory_planner_B.minmn +
6352 cartesian_trajectory_planner_B.b_i_fn];
6353 }
6354
6355 for (cartesian_trajectory_planner_B.minmana =
6356 cartesian_trajectory_planner_B.rankR;
6357 cartesian_trajectory_planner_B.minmana >= 1;
6358 cartesian_trajectory_planner_B.minmana--) {
6359 Y->data[(b_jpvt->data[cartesian_trajectory_planner_B.minmana - 1] +
6360 Y->size[0] * cartesian_trajectory_planner_B.minmn) - 1] /=
6361 c_A->data[((cartesian_trajectory_planner_B.minmana - 1) * c_A->size[0]
6362 + cartesian_trajectory_planner_B.minmana) - 1];
6363 cartesian_trajectory_planner_B.na =
6364 cartesian_trajectory_planner_B.minmana - 2;
6365 for (cartesian_trajectory_planner_B.b_i_fn = 0;
6366 cartesian_trajectory_planner_B.b_i_fn <=
6367 cartesian_trajectory_planner_B.na;
6368 cartesian_trajectory_planner_B.b_i_fn++) {
6369 Y->data[(b_jpvt->data[cartesian_trajectory_planner_B.b_i_fn] + Y->
6370 size[0] * cartesian_trajectory_planner_B.minmn) - 1] -=
6371 Y->data[(b_jpvt->data[cartesian_trajectory_planner_B.minmana - 1] +
6372 Y->size[0] * cartesian_trajectory_planner_B.minmn) - 1] *
6373 c_A->data[(cartesian_trajectory_planner_B.minmana - 1) * c_A->size[0]
6374 + cartesian_trajectory_planner_B.b_i_fn];
6375 }
6376 }
6377 }
6378 }
6379
6380 cartesian_traje_emxFree_int32_T(&b_jpvt_0);
6381 cartesian_trajec_emxFree_real_T(&B_0);
6382 cartesian_traje_emxFree_int32_T(&b_jpvt);
6383 cartesian_trajec_emxFree_real_T(&b_tau);
6384 cartesian_trajec_emxFree_real_T(&c_A);
6385}
6386
6387static void cartesian_tra_emxFree_boolean_T(emxArray_boolean_T_cartesian__T
6388 **pEmxArray)
6389{
6390 if (*pEmxArray != (emxArray_boolean_T_cartesian__T *)NULL) {
6391 if (((*pEmxArray)->data != (boolean_T *)NULL) && (*pEmxArray)->canFreeData)
6392 {
6393 free((*pEmxArray)->data);
6394 }
6395
6396 free((*pEmxArray)->size);
6397 free(*pEmxArray);
6398 *pEmxArray = (emxArray_boolean_T_cartesian__T *)NULL;
6399 }
6400}
6401
6402static boolean_T DampedBFGSwGradientProjection_a(const
6403 h_robotics_core_internal_Damp_T *obj, const real_T Hg[6], const
6404 emxArray_real_T_cartesian_tra_T *alpha)
6405{
6406 boolean_T flag;
6407 boolean_T y;
6408 emxArray_boolean_T_cartesian__T *x;
6409 int32_T ix;
6410 int32_T loop_ub;
6411 boolean_T exitg1;
6412 cartesian_tra_emxInit_boolean_T(&x, 1);
6413 if (cartesian_trajectory_pla_norm_a(Hg) < obj->GradientTolerance) {
6414 ix = x->size[0];
6415 x->size[0] = alpha->size[0];
6416 car_emxEnsureCapacity_boolean_T(x, ix);
6417 loop_ub = alpha->size[0];
6418 for (ix = 0; ix < loop_ub; ix++) {
6419 x->data[ix] = (alpha->data[ix] <= 0.0);
6420 }
6421
6422 y = true;
6423 ix = 0;
6424 exitg1 = false;
6425 while ((!exitg1) && (ix + 1 <= x->size[0])) {
6426 if (!x->data[ix]) {
6427 y = false;
6428 exitg1 = true;
6429 } else {
6430 ix++;
6431 }
6432 }
6433
6434 if (y) {
6435 flag = true;
6436 } else {
6437 flag = false;
6438 }
6439 } else {
6440 flag = false;
6441 }
6442
6443 cartesian_tra_emxFree_boolean_T(&x);
6444 return flag;
6445}
6446
6447static void cartesian_trajectory_planne_inv(const
6448 emxArray_real_T_cartesian_tra_T *x, emxArray_real_T_cartesian_tra_T *y)
6449{
6450 int32_T n;
6451 emxArray_int32_T_cartesian_tr_T *p;
6452 int32_T c;
6453 emxArray_real_T_cartesian_tra_T *c_A;
6454 emxArray_int32_T_cartesian_tr_T *b_ipiv;
6455 int32_T info;
6456 int32_T n_0;
6457 int32_T yk;
6458 emxArray_real_T_cartesian_tra_T *y_0;
6459 if ((x->size[0] == 0) || (x->size[1] == 0)) {
6460 info = y->size[0] * y->size[1];
6461 y->size[0] = x->size[0];
6462 y->size[1] = x->size[1];
6463 cartes_emxEnsureCapacity_real_T(y, info);
6464 n_0 = x->size[0] * x->size[1] - 1;
6465 for (info = 0; info <= n_0; info++) {
6466 y->data[info] = x->data[info];
6467 }
6468 } else {
6469 n = x->size[0];
6470 info = y->size[0] * y->size[1];
6471 y->size[0] = x->size[0];
6472 y->size[1] = x->size[1];
6473 cartes_emxEnsureCapacity_real_T(y, info);
6474 n_0 = x->size[0] * x->size[1] - 1;
6475 for (info = 0; info <= n_0; info++) {
6476 y->data[info] = 0.0;
6477 }
6478
6479 cartesian_traje_emxInit_int32_T(&p, 2);
6480 cartesian_trajec_emxInit_real_T(&c_A, 2);
6481 cartesian_traje_emxInit_int32_T(&b_ipiv, 2);
6482 cartesian_trajectory_pl_xzgetrf(x->size[0], x->size[0], x, x->size[0], c_A,
6483 b_ipiv, &info);
6484 if (x->size[0] < 1) {
6485 n_0 = 0;
6486 } else {
6487 n_0 = x->size[0];
6488 }
6489
6490 info = p->size[0] * p->size[1];
6491 p->size[0] = 1;
6492 p->size[1] = n_0;
6493 carte_emxEnsureCapacity_int32_T(p, info);
6494 if (n_0 > 0) {
6495 p->data[0] = 1;
6496 yk = 1;
6497 for (info = 2; info <= n_0; info++) {
6498 yk++;
6499 p->data[info - 1] = yk;
6500 }
6501 }
6502
6503 n_0 = b_ipiv->size[1] - 1;
6504 for (info = 0; info <= n_0; info++) {
6505 if (b_ipiv->data[info] > static_cast<real_T>(info) + 1.0) {
6506 yk = p->data[b_ipiv->data[info] - 1];
6507 p->data[b_ipiv->data[info] - 1] = p->data[info];
6508 p->data[info] = yk;
6509 }
6510 }
6511
6512 cartesian_traje_emxFree_int32_T(&b_ipiv);
6513 for (info = 0; info < n; info++) {
6514 c = p->data[info] - 1;
6515 y->data[info + y->size[0] * (p->data[info] - 1)] = 1.0;
6516 for (n_0 = info + 1; n_0 <= n; n_0++) {
6517 if (y->data[(y->size[0] * c + n_0) - 1] != 0.0) {
6518 for (yk = n_0 + 1; yk <= n; yk++) {
6519 y->data[(yk + y->size[0] * c) - 1] -= c_A->data[((n_0 - 1) *
6520 c_A->size[0] + yk) - 1] * y->data[(y->size[0] * c + n_0) - 1];
6521 }
6522 }
6523 }
6524 }
6525
6526 cartesian_traje_emxFree_int32_T(&p);
6527 cartesian_trajec_emxInit_real_T(&y_0, 2);
6528 info = y_0->size[0] * y_0->size[1];
6529 y_0->size[0] = y->size[0];
6530 y_0->size[1] = y->size[1];
6531 cartes_emxEnsureCapacity_real_T(y_0, info);
6532 n_0 = y->size[0] * y->size[1];
6533 for (info = 0; info < n_0; info++) {
6534 y_0->data[info] = y->data[info];
6535 }
6536
6537 cartesian_trajectory_plan_xtrsm(x->size[0], x->size[0], c_A, x->size[0], y_0,
6538 x->size[0], y);
6539 cartesian_trajec_emxFree_real_T(&y_0);
6540 cartesian_trajec_emxFree_real_T(&c_A);
6541 }
6542}
6543
6544static void cartesian_trajectory_plann_diag(const
6545 emxArray_real_T_cartesian_tra_T *v, emxArray_real_T_cartesian_tra_T *d)
6546{
6547 int32_T u0;
6548 int32_T u1;
6549 if ((v->size[0] == 1) && (v->size[1] == 1)) {
6550 u0 = d->size[0];
6551 d->size[0] = 1;
6552 cartes_emxEnsureCapacity_real_T(d, u0);
6553 d->data[0] = v->data[0];
6554 } else {
6555 if (0 < v->size[1]) {
6556 u0 = v->size[0];
6557 u1 = v->size[1];
6558 if (u0 < u1) {
6559 u1 = u0;
6560 }
6561 } else {
6562 u1 = 0;
6563 }
6564
6565 u0 = d->size[0];
6566 d->size[0] = u1;
6567 cartes_emxEnsureCapacity_real_T(d, u0);
6568 for (u0 = 0; u0 < u1; u0++) {
6569 d->data[u0] = v->data[v->size[0] * u0 + u0];
6570 }
6571 }
6572}
6573
6574static void cartesian_trajectory_pl_sqrt_as(emxArray_real_T_cartesian_tra_T *x)
6575{
6576 int32_T nx;
6577 int32_T b_k;
6578 nx = x->size[0] - 1;
6579 for (b_k = 0; b_k <= nx; b_k++) {
6580 x->data[b_k] = sqrt(x->data[b_k]);
6581 }
6582}
6583
6584static boolean_T cartesian_trajectory_planne_any(const
6585 emxArray_boolean_T_cartesian__T *x)
6586{
6587 boolean_T y;
6588 int32_T ix;
6589 boolean_T exitg1;
6590 y = false;
6591 ix = 0;
6592 exitg1 = false;
6593 while ((!exitg1) && (ix + 1 <= x->size[0])) {
6594 if (!x->data[ix]) {
6595 ix++;
6596 } else {
6597 y = true;
6598 exitg1 = true;
6599 }
6600 }
6601
6602 return y;
6603}
6604
6605static boolean_T cartesian_tr_isPositiveDefinite(const real_T B[36])
6606{
6607 emxArray_real_T_cartesian_tra_T *b_x;
6608 boolean_T exitg1;
6609 cartesian_trajectory_planner_B.c_A_size_idx_0 = 6;
6610 cartesian_trajectory_planner_B.c_A_size_idx_1 = 6;
6611 memcpy(&cartesian_trajectory_planner_B.c_A_data[0], &B[0], 36U * sizeof(real_T));
6612 cartesian_trajectory_planner_B.b_info = 0;
6613 cartesian_trajectory_planner_B.b_j_jk = 1;
6614 cartesian_trajec_emxInit_real_T(&b_x, 2);
6615 exitg1 = false;
6616 while ((!exitg1) && (cartesian_trajectory_planner_B.b_j_jk - 1 < 6)) {
6617 cartesian_trajectory_planner_B.jm1 = cartesian_trajectory_planner_B.b_j_jk -
6618 2;
6619 cartesian_trajectory_planner_B.idxAjj =
6620 ((cartesian_trajectory_planner_B.b_j_jk - 1) * 6 +
6621 cartesian_trajectory_planner_B.b_j_jk) - 1;
6622 cartesian_trajectory_planner_B.ssq = 0.0;
6623 if (cartesian_trajectory_planner_B.b_j_jk - 1 >= 1) {
6624 cartesian_trajectory_planner_B.ix_h =
6625 cartesian_trajectory_planner_B.b_j_jk - 1;
6626 cartesian_trajectory_planner_B.iy = cartesian_trajectory_planner_B.b_j_jk
6627 - 1;
6628 for (cartesian_trajectory_planner_B.k_d = 0;
6629 cartesian_trajectory_planner_B.k_d <=
6630 cartesian_trajectory_planner_B.jm1;
6631 cartesian_trajectory_planner_B.k_d++) {
6632 cartesian_trajectory_planner_B.ssq +=
6633 cartesian_trajectory_planner_B.c_A_data[cartesian_trajectory_planner_B.ix_h]
6634 * cartesian_trajectory_planner_B.c_A_data[cartesian_trajectory_planner_B.iy];
6635 cartesian_trajectory_planner_B.ix_h += 6;
6636 cartesian_trajectory_planner_B.iy += 6;
6637 }
6638 }
6639
6640 cartesian_trajectory_planner_B.ssq =
6641 cartesian_trajectory_planner_B.c_A_data[cartesian_trajectory_planner_B.idxAjj]
6642 - cartesian_trajectory_planner_B.ssq;
6643 if (cartesian_trajectory_planner_B.ssq > 0.0) {
6644 cartesian_trajectory_planner_B.ssq = sqrt
6645 (cartesian_trajectory_planner_B.ssq);
6646 cartesian_trajectory_planner_B.c_A_data[cartesian_trajectory_planner_B.idxAjj]
6647 = cartesian_trajectory_planner_B.ssq;
6648 if (cartesian_trajectory_planner_B.b_j_jk < 6) {
6649 if (cartesian_trajectory_planner_B.b_j_jk - 1 != 0) {
6650 cartesian_trajectory_planner_B.ix_h =
6651 cartesian_trajectory_planner_B.b_j_jk - 1;
6652 cartesian_trajectory_planner_B.jm1 =
6653 (cartesian_trajectory_planner_B.b_j_jk - 2) * 6 +
6654 cartesian_trajectory_planner_B.b_j_jk;
6655 for (cartesian_trajectory_planner_B.k_d =
6656 cartesian_trajectory_planner_B.b_j_jk + 1;
6657 cartesian_trajectory_planner_B.k_d <=
6658 cartesian_trajectory_planner_B.jm1 + 1;
6659 cartesian_trajectory_planner_B.k_d += 6) {
6660 cartesian_trajectory_planner_B.c =
6661 -cartesian_trajectory_planner_B.c_A_data[cartesian_trajectory_planner_B.ix_h];
6662 cartesian_trajectory_planner_B.iy =
6663 cartesian_trajectory_planner_B.idxAjj + 1;
6664 cartesian_trajectory_planner_B.d_k =
6665 cartesian_trajectory_planner_B.k_d -
6666 cartesian_trajectory_planner_B.b_j_jk;
6667 for (cartesian_trajectory_planner_B.ia =
6668 cartesian_trajectory_planner_B.k_d;
6669 cartesian_trajectory_planner_B.ia <=
6670 cartesian_trajectory_planner_B.d_k + 5;
6671 cartesian_trajectory_planner_B.ia++) {
6672 cartesian_trajectory_planner_B.c_A_data[cartesian_trajectory_planner_B.iy]
6673 +=
6674 cartesian_trajectory_planner_B.c_A_data[cartesian_trajectory_planner_B.ia
6675 - 1] * cartesian_trajectory_planner_B.c;
6676 cartesian_trajectory_planner_B.iy++;
6677 }
6678
6679 cartesian_trajectory_planner_B.ix_h += 6;
6680 }
6681 }
6682
6683 cartesian_trajectory_planner_B.ssq = 1.0 /
6684 cartesian_trajectory_planner_B.ssq;
6685 cartesian_trajectory_planner_B.ix_h = b_x->size[0] * b_x->size[1];
6686 b_x->size[0] = 6;
6687 b_x->size[1] = 6;
6688 cartes_emxEnsureCapacity_real_T(b_x, cartesian_trajectory_planner_B.ix_h);
6689 cartesian_trajectory_planner_B.c_A_size_idx_0 =
6690 cartesian_trajectory_planner_B.c_A_size_idx_0 *
6691 cartesian_trajectory_planner_B.c_A_size_idx_1 - 1;
6692 for (cartesian_trajectory_planner_B.ix_h = 0;
6693 cartesian_trajectory_planner_B.ix_h <=
6694 cartesian_trajectory_planner_B.c_A_size_idx_0;
6695 cartesian_trajectory_planner_B.ix_h++) {
6696 b_x->data[cartesian_trajectory_planner_B.ix_h] =
6697 cartesian_trajectory_planner_B.c_A_data[cartesian_trajectory_planner_B.ix_h];
6698 }
6699
6700 cartesian_trajectory_planner_B.jm1 =
6701 cartesian_trajectory_planner_B.idxAjj -
6702 cartesian_trajectory_planner_B.b_j_jk;
6703 for (cartesian_trajectory_planner_B.k_d =
6704 cartesian_trajectory_planner_B.idxAjj + 2;
6705 cartesian_trajectory_planner_B.k_d <=
6706 cartesian_trajectory_planner_B.jm1 + 7;
6707 cartesian_trajectory_planner_B.k_d++) {
6708 b_x->data[cartesian_trajectory_planner_B.k_d - 1] *=
6709 cartesian_trajectory_planner_B.ssq;
6710 }
6711
6712 cartesian_trajectory_planner_B.c_A_size_idx_0 = b_x->size[0];
6713 cartesian_trajectory_planner_B.c_A_size_idx_1 = b_x->size[1];
6714 for (cartesian_trajectory_planner_B.ix_h = 0;
6715 cartesian_trajectory_planner_B.ix_h < 36;
6716 cartesian_trajectory_planner_B.ix_h++) {
6717 cartesian_trajectory_planner_B.c_A_data[cartesian_trajectory_planner_B.ix_h]
6718 = b_x->data[cartesian_trajectory_planner_B.ix_h];
6719 }
6720 }
6721
6722 cartesian_trajectory_planner_B.b_j_jk++;
6723 } else {
6724 cartesian_trajectory_planner_B.b_info =
6725 cartesian_trajectory_planner_B.b_j_jk;
6726 exitg1 = true;
6727 }
6728 }
6729
6730 cartesian_trajec_emxFree_real_T(&b_x);
6731 return cartesian_trajectory_planner_B.b_info == 0;
6732}
6733
6734static boolean_T DampedBFGSwGradientProjectio_as(const
6735 h_robotics_core_internal_Damp_T *obj, const real_T xNew[6])
6736{
6737 boolean_T flag;
6738 emxArray_real_T_cartesian_tra_T *b;
6739 emxArray_real_T_cartesian_tra_T *c;
6740 int32_T m;
6741 int32_T inner;
6742 int32_T b_i;
6743 int32_T loop_ub;
6744 emxArray_boolean_T_cartesian__T *c_0;
6745 cartesian_trajec_emxInit_real_T(&b, 2);
6746 cartesian_trajec_emxInit_real_T(&c, 1);
6747 cartesian_tra_emxInit_boolean_T(&c_0, 1);
6748 if (obj->ConstraintsOn) {
6749 b_i = b->size[0] * b->size[1];
6750 b->size[0] = obj->ConstraintMatrix->size[0];
6751 b->size[1] = obj->ConstraintMatrix->size[1];
6752 cartes_emxEnsureCapacity_real_T(b, b_i);
6753 loop_ub = obj->ConstraintMatrix->size[0] * obj->ConstraintMatrix->size[1] -
6754 1;
6755 for (b_i = 0; b_i <= loop_ub; b_i++) {
6756 b->data[b_i] = obj->ConstraintMatrix->data[b_i];
6757 }
6758
6759 m = b->size[1] - 1;
6760 inner = b->size[0] - 1;
6761 b_i = c->size[0];
6762 c->size[0] = b->size[1];
6763 cartes_emxEnsureCapacity_real_T(c, b_i);
6764 for (b_i = 0; b_i <= m; b_i++) {
6765 c->data[b_i] = 0.0;
6766 }
6767
6768 for (b_i = 0; b_i <= inner; b_i++) {
6769 for (loop_ub = 0; loop_ub <= m; loop_ub++) {
6770 c->data[loop_ub] += b->data[loop_ub * b->size[0] + b_i] * xNew[b_i];
6771 }
6772 }
6773
6774 b_i = c_0->size[0];
6775 c_0->size[0] = c->size[0];
6776 car_emxEnsureCapacity_boolean_T(c_0, b_i);
6777 loop_ub = c->size[0];
6778 for (b_i = 0; b_i < loop_ub; b_i++) {
6779 c_0->data[b_i] = (c->data[b_i] - obj->ConstraintBound->data[b_i] >
6780 1.4901161193847656E-8);
6781 }
6782
6783 if (cartesian_trajectory_planne_any(c_0)) {
6784 flag = true;
6785 } else {
6786 flag = false;
6787 }
6788 } else {
6789 flag = false;
6790 }
6791
6792 cartesian_tra_emxFree_boolean_T(&c_0);
6793 cartesian_trajec_emxFree_real_T(&c);
6794 cartesian_trajec_emxFree_real_T(&b);
6795 return flag;
6796}
6797
6798static void DampedBFGSwGradientProjection_s(h_robotics_core_internal_Damp_T *obj,
6799 real_T xSol[6], c_robotics_core_internal_NLPS_T *exitFlag, real_T *err, real_T
6800 *iter)
6801{
6802 emxArray_real_T_cartesian_tra_T *unusedU1;
6803 emxArray_real_T_cartesian_tra_T *grad;
6804 emxArray_boolean_T_cartesian__T *activeSet;
6805 emxArray_real_T_cartesian_tra_T *A;
6806 emxArray_real_T_cartesian_tra_T *alpha;
6807 emxArray_real_T_cartesian_tra_T *AIn;
6808 emxArray_real_T_cartesian_tra_T *L;
6809 f_robotics_manip_internal_IKE_T *b;
6810 f_robotics_manip_internal_IKE_T *c;
6811 f_robotics_manip_internal_IKE_T *d;
6812 emxArray_int32_T_cartesian_tr_T *cb;
6813 emxArray_int32_T_cartesian_tr_T *db;
6814 emxArray_int32_T_cartesian_tr_T *eb;
6815 emxArray_int32_T_cartesian_tr_T *fb;
6816 emxArray_int32_T_cartesian_tr_T *gb;
6817 f_robotics_manip_internal_IKE_T *args;
6818 emxArray_real_T_cartesian_tra_T *a;
6819 emxArray_int32_T_cartesian_tr_T *ii;
6820 emxArray_real_T_cartesian_tra_T *y;
6821 emxArray_int32_T_cartesian_tr_T *ii_0;
6822 emxArray_real_T_cartesian_tra_T *y_0;
6823 emxArray_boolean_T_cartesian__T *x;
6824 emxArray_real_T_cartesian_tra_T *A_0;
6825 emxArray_real_T_cartesian_tra_T *A_1;
6826 emxArray_real_T_cartesian_tra_T *A_2;
6827 emxArray_real_T_cartesian_tra_T *sigma;
6828 emxArray_real_T_cartesian_tra_T *tmp;
6829 emxArray_real_T_cartesian_tra_T *tmp_0;
6830 emxArray_real_T_cartesian_tra_T *grad_0;
6831 emxArray_real_T_cartesian_tra_T *sNew;
6832 emxArray_int32_T_cartesian_tr_T *ii_1;
6833 emxArray_int32_T_cartesian_tr_T *ii_2;
6834 emxArray_real_T_cartesian_tra_T *grad_1;
6835 emxArray_real_T_cartesian_tra_T *A_3;
6836 emxArray_real_T_cartesian_tra_T *alpha_0;
6837 emxArray_int32_T_cartesian_tr_T *ii_3;
6838 static const int8_T tmp_1[36] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1,
6839 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1 };
6840
6841 int32_T exitg1;
6842 int32_T exitg2;
6843 boolean_T exitg3;
6844 boolean_T guard1 = false;
6845 boolean_T guard2 = false;
6846 for (cartesian_trajectory_planner_B.i_e = 0;
6847 cartesian_trajectory_planner_B.i_e < 6;
6848 cartesian_trajectory_planner_B.i_e++) {
6849 cartesian_trajectory_planner_B.x[cartesian_trajectory_planner_B.i_e] =
6850 obj->SeedInternal[cartesian_trajectory_planner_B.i_e];
6851 }
6852
6853 cartesian_trajec_emxInit_real_T(&unusedU1, 2);
6854 cartesian_trajec_emxInit_real_T(&grad, 1);
6855 obj->TimeObjInternal.StartTime = ctimefun();
6856 cartesian_IKHelpers_computeCost(cartesian_trajectory_planner_B.x,
6857 obj->ExtraArgs, &cartesian_trajectory_planner_B.cost,
6858 cartesian_trajectory_planner_B.unusedU0, unusedU1, &b);
6859 obj->ExtraArgs = b;
6860 args = obj->ExtraArgs;
6861 cartesian_trajectory_planner_B.b_i = grad->size[0];
6862 grad->size[0] = args->GradTemp->size[0];
6863 cartes_emxEnsureCapacity_real_T(grad, cartesian_trajectory_planner_B.b_i);
6864 cartesian_trajectory_planner_B.i_e = args->GradTemp->size[0];
6865 for (cartesian_trajectory_planner_B.b_i = 0;
6866 cartesian_trajectory_planner_B.b_i < cartesian_trajectory_planner_B.i_e;
6867 cartesian_trajectory_planner_B.b_i++) {
6868 grad->data[cartesian_trajectory_planner_B.b_i] = args->GradTemp->
6869 data[cartesian_trajectory_planner_B.b_i];
6870 }
6871
6872 cartesian_trajectory_planne_eye(cartesian_trajectory_planner_B.unusedU0);
6873 memcpy(&cartesian_trajectory_planner_B.H[0],
6874 &cartesian_trajectory_planner_B.unusedU0[0], 36U * sizeof(real_T));
6875 cartesian_tra_emxInit_boolean_T(&activeSet, 1);
6876 cartesian_trajec_emxInit_real_T(&A, 2);
6877 cartesian_trajec_emxInit_real_T(&alpha, 1);
6878 cartesian_traje_emxInit_int32_T(&ii, 1);
6879 if (obj->ConstraintsOn) {
6880 cartesian_trajectory_planner_B.b_i = A->size[0] * A->size[1];
6881 A->size[0] = obj->ConstraintMatrix->size[0];
6882 A->size[1] = obj->ConstraintMatrix->size[1];
6883 cartes_emxEnsureCapacity_real_T(A, cartesian_trajectory_planner_B.b_i);
6884 cartesian_trajectory_planner_B.i_e = obj->ConstraintMatrix->size[0] *
6885 obj->ConstraintMatrix->size[1] - 1;
6886 for (cartesian_trajectory_planner_B.b_i = 0;
6887 cartesian_trajectory_planner_B.b_i <=
6888 cartesian_trajectory_planner_B.i_e; cartesian_trajectory_planner_B.b_i
6889 ++) {
6890 A->data[cartesian_trajectory_planner_B.b_i] = obj->ConstraintMatrix->
6891 data[cartesian_trajectory_planner_B.b_i];
6892 }
6893
6894 cartesian_trajectory_planner_B.m_p = A->size[1] - 1;
6895 cartesian_trajectory_planner_B.inner = A->size[0] - 1;
6896 cartesian_trajectory_planner_B.b_i = alpha->size[0];
6897 alpha->size[0] = A->size[1];
6898 cartes_emxEnsureCapacity_real_T(alpha, cartesian_trajectory_planner_B.b_i);
6899 for (cartesian_trajectory_planner_B.b_i = 0;
6900 cartesian_trajectory_planner_B.b_i <=
6901 cartesian_trajectory_planner_B.m_p; cartesian_trajectory_planner_B.b_i
6902 ++) {
6903 alpha->data[cartesian_trajectory_planner_B.b_i] = 0.0;
6904 }
6905
6906 for (cartesian_trajectory_planner_B.nx_i = 0;
6907 cartesian_trajectory_planner_B.nx_i <=
6908 cartesian_trajectory_planner_B.inner;
6909 cartesian_trajectory_planner_B.nx_i++) {
6910 for (cartesian_trajectory_planner_B.g_idx_0 = 0;
6911 cartesian_trajectory_planner_B.g_idx_0 <=
6912 cartesian_trajectory_planner_B.m_p;
6913 cartesian_trajectory_planner_B.g_idx_0++) {
6914 alpha->data[cartesian_trajectory_planner_B.g_idx_0] += A->
6915 data[cartesian_trajectory_planner_B.g_idx_0 * A->size[0] +
6916 cartesian_trajectory_planner_B.nx_i] *
6917 cartesian_trajectory_planner_B.x[cartesian_trajectory_planner_B.nx_i];
6918 }
6919 }
6920
6921 cartesian_trajectory_planner_B.b_i = activeSet->size[0];
6922 activeSet->size[0] = alpha->size[0];
6923 car_emxEnsureCapacity_boolean_T(activeSet,
6924 cartesian_trajectory_planner_B.b_i);
6925 cartesian_trajectory_planner_B.i_e = alpha->size[0];
6926 for (cartesian_trajectory_planner_B.b_i = 0;
6927 cartesian_trajectory_planner_B.b_i < cartesian_trajectory_planner_B.i_e;
6928 cartesian_trajectory_planner_B.b_i++) {
6929 activeSet->data[cartesian_trajectory_planner_B.b_i] = (alpha->
6930 data[cartesian_trajectory_planner_B.b_i] >= obj->ConstraintBound->
6931 data[cartesian_trajectory_planner_B.b_i]);
6932 }
6933
6934 cartesian_trajectory_planner_B.nx_i = activeSet->size[0] - 1;
6935 cartesian_trajectory_planner_B.idx = 0;
6936 for (cartesian_trajectory_planner_B.g_idx_0 = 0;
6937 cartesian_trajectory_planner_B.g_idx_0 <=
6938 cartesian_trajectory_planner_B.nx_i;
6939 cartesian_trajectory_planner_B.g_idx_0++) {
6940 if (activeSet->data[cartesian_trajectory_planner_B.g_idx_0]) {
6941 cartesian_trajectory_planner_B.idx++;
6942 }
6943 }
6944
6945 cartesian_trajectory_planner_B.b_i = ii->size[0];
6946 ii->size[0] = cartesian_trajectory_planner_B.idx;
6947 carte_emxEnsureCapacity_int32_T(ii, cartesian_trajectory_planner_B.b_i);
6948 cartesian_trajectory_planner_B.b_i = 0;
6949 for (cartesian_trajectory_planner_B.g_idx_0 = 0;
6950 cartesian_trajectory_planner_B.g_idx_0 <=
6951 cartesian_trajectory_planner_B.nx_i;
6952 cartesian_trajectory_planner_B.g_idx_0++) {
6953 if (activeSet->data[cartesian_trajectory_planner_B.g_idx_0]) {
6954 ii->data[cartesian_trajectory_planner_B.b_i] =
6955 cartesian_trajectory_planner_B.g_idx_0 + 1;
6956 cartesian_trajectory_planner_B.b_i++;
6957 }
6958 }
6959
6960 cartesian_trajectory_planner_B.i_e = obj->ConstraintMatrix->size[0];
6961 cartesian_trajectory_planner_B.b_i = A->size[0] * A->size[1];
6962 A->size[0] = cartesian_trajectory_planner_B.i_e;
6963 A->size[1] = ii->size[0];
6964 cartes_emxEnsureCapacity_real_T(A, cartesian_trajectory_planner_B.b_i);
6965 cartesian_trajectory_planner_B.n_f = ii->size[0];
6966 for (cartesian_trajectory_planner_B.b_i = 0;
6967 cartesian_trajectory_planner_B.b_i < cartesian_trajectory_planner_B.n_f;
6968 cartesian_trajectory_planner_B.b_i++) {
6969 for (cartesian_trajectory_planner_B.idx = 0;
6970 cartesian_trajectory_planner_B.idx <
6971 cartesian_trajectory_planner_B.i_e;
6972 cartesian_trajectory_planner_B.idx++) {
6973 A->data[cartesian_trajectory_planner_B.idx + A->size[0] *
6974 cartesian_trajectory_planner_B.b_i] = obj->ConstraintMatrix->data
6975 [(ii->data[cartesian_trajectory_planner_B.b_i] - 1) *
6976 obj->ConstraintMatrix->size[0] + cartesian_trajectory_planner_B.idx];
6977 }
6978 }
6979 } else {
6980 cartesian_trajectory_planner_B.g_idx_0 = obj->ConstraintBound->size[0];
6981 cartesian_trajectory_planner_B.b_i = activeSet->size[0];
6982 activeSet->size[0] = cartesian_trajectory_planner_B.g_idx_0;
6983 car_emxEnsureCapacity_boolean_T(activeSet,
6984 cartesian_trajectory_planner_B.b_i);
6985 for (cartesian_trajectory_planner_B.b_i = 0;
6986 cartesian_trajectory_planner_B.b_i <
6987 cartesian_trajectory_planner_B.g_idx_0;
6988 cartesian_trajectory_planner_B.b_i++) {
6989 activeSet->data[cartesian_trajectory_planner_B.b_i] = false;
6990 }
6991
6992 A->size[0] = 6;
6993 A->size[1] = 0;
6994 }
6995
6996 cartesian_trajectory_planner_B.j = A->size[1] - 1;
6997 cartesian_trajec_emxInit_real_T(&AIn, 2);
6998 cartesian_trajec_emxInit_real_T(&A_0, 2);
6999 cartesian_trajec_emxInit_real_T(&A_1, 1);
7000 for (cartesian_trajectory_planner_B.nx_i = 0;
7001 cartesian_trajectory_planner_B.nx_i <= cartesian_trajectory_planner_B.j;
7002 cartesian_trajectory_planner_B.nx_i++) {
7003 cartesian_trajectory_planner_B.i_e = A->size[0];
7004 cartesian_trajectory_planner_B.b_i = A_0->size[0] * A_0->size[1];
7005 A_0->size[0] = 1;
7006 A_0->size[1] = cartesian_trajectory_planner_B.i_e;
7007 cartes_emxEnsureCapacity_real_T(A_0, cartesian_trajectory_planner_B.b_i);
7008 for (cartesian_trajectory_planner_B.b_i = 0;
7009 cartesian_trajectory_planner_B.b_i < cartesian_trajectory_planner_B.i_e;
7010 cartesian_trajectory_planner_B.b_i++) {
7011 A_0->data[cartesian_trajectory_planner_B.b_i] = A->data[A->size[0] *
7012 cartesian_trajectory_planner_B.nx_i + cartesian_trajectory_planner_B.b_i];
7013 }
7014
7015 cartesian_trajectory_planner_B.i_e = A->size[0];
7016 cartesian_trajectory_planner_B.b_i = A_1->size[0];
7017 A_1->size[0] = cartesian_trajectory_planner_B.i_e;
7018 cartes_emxEnsureCapacity_real_T(A_1, cartesian_trajectory_planner_B.b_i);
7019 for (cartesian_trajectory_planner_B.b_i = 0;
7020 cartesian_trajectory_planner_B.b_i < cartesian_trajectory_planner_B.i_e;
7021 cartesian_trajectory_planner_B.b_i++) {
7022 A_1->data[cartesian_trajectory_planner_B.b_i] = A->data[A->size[0] *
7023 cartesian_trajectory_planner_B.nx_i + cartesian_trajectory_planner_B.b_i];
7024 }
7025
7026 cartesian_trajectory_planner_B.A_i = 0.0;
7027 for (cartesian_trajectory_planner_B.b_i = 0;
7028 cartesian_trajectory_planner_B.b_i < 6;
7029 cartesian_trajectory_planner_B.b_i++) {
7030 cartesian_trajectory_planner_B.Hg[cartesian_trajectory_planner_B.b_i] =
7031 0.0;
7032 for (cartesian_trajectory_planner_B.idx = 0;
7033 cartesian_trajectory_planner_B.idx < 6;
7034 cartesian_trajectory_planner_B.idx++) {
7035 cartesian_trajectory_planner_B.s_j = cartesian_trajectory_planner_B.H[6 *
7036 cartesian_trajectory_planner_B.b_i +
7037 cartesian_trajectory_planner_B.idx] * A_0->
7038 data[cartesian_trajectory_planner_B.idx] +
7039 cartesian_trajectory_planner_B.Hg[cartesian_trajectory_planner_B.b_i];
7040 cartesian_trajectory_planner_B.Hg[cartesian_trajectory_planner_B.b_i] =
7041 cartesian_trajectory_planner_B.s_j;
7042 }
7043
7044 cartesian_trajectory_planner_B.A_i +=
7045 cartesian_trajectory_planner_B.Hg[cartesian_trajectory_planner_B.b_i] *
7046 A_1->data[cartesian_trajectory_planner_B.b_i];
7047 }
7048
7049 cartesian_trajectory_planner_B.s_j = 1.0 /
7050 cartesian_trajectory_planner_B.A_i;
7051 for (cartesian_trajectory_planner_B.b_i = 0;
7052 cartesian_trajectory_planner_B.b_i < 36;
7053 cartesian_trajectory_planner_B.b_i++) {
7054 cartesian_trajectory_planner_B.V[cartesian_trajectory_planner_B.b_i] =
7055 cartesian_trajectory_planner_B.s_j *
7056 cartesian_trajectory_planner_B.H[cartesian_trajectory_planner_B.b_i];
7057 }
7058
7059 cartesian_trajectory_planner_B.i_e = A->size[0];
7060 cartesian_trajectory_planner_B.n_f = A->size[0];
7061 cartesian_trajectory_planner_B.b_i = AIn->size[0] * AIn->size[1];
7062 AIn->size[0] = cartesian_trajectory_planner_B.i_e;
7063 AIn->size[1] = cartesian_trajectory_planner_B.n_f;
7064 cartes_emxEnsureCapacity_real_T(AIn, cartesian_trajectory_planner_B.b_i);
7065 for (cartesian_trajectory_planner_B.b_i = 0;
7066 cartesian_trajectory_planner_B.b_i < cartesian_trajectory_planner_B.n_f;
7067 cartesian_trajectory_planner_B.b_i++) {
7068 for (cartesian_trajectory_planner_B.idx = 0;
7069 cartesian_trajectory_planner_B.idx <
7070 cartesian_trajectory_planner_B.i_e;
7071 cartesian_trajectory_planner_B.idx++) {
7072 AIn->data[cartesian_trajectory_planner_B.idx + AIn->size[0] *
7073 cartesian_trajectory_planner_B.b_i] = A->data[A->size[0] *
7074 cartesian_trajectory_planner_B.nx_i +
7075 cartesian_trajectory_planner_B.idx] * A->data[A->size[0] *
7076 cartesian_trajectory_planner_B.nx_i +
7077 cartesian_trajectory_planner_B.b_i];
7078 }
7079 }
7080
7081 cartesian_trajectory_planner_B.n_f = AIn->size[1] - 1;
7082 cartesian_trajectory_planner_B.b_i = unusedU1->size[0] * unusedU1->size[1];
7083 unusedU1->size[0] = 6;
7084 unusedU1->size[1] = AIn->size[1];
7085 cartes_emxEnsureCapacity_real_T(unusedU1, cartesian_trajectory_planner_B.b_i);
7086 for (cartesian_trajectory_planner_B.idx = 0;
7087 cartesian_trajectory_planner_B.idx <=
7088 cartesian_trajectory_planner_B.n_f; cartesian_trajectory_planner_B.idx
7089 ++) {
7090 cartesian_trajectory_planner_B.coffset =
7091 cartesian_trajectory_planner_B.idx * 6 - 1;
7092 cartesian_trajectory_planner_B.boffset =
7093 cartesian_trajectory_planner_B.idx * AIn->size[0] - 1;
7094 for (cartesian_trajectory_planner_B.b_i = 0;
7095 cartesian_trajectory_planner_B.b_i < 6;
7096 cartesian_trajectory_planner_B.b_i++) {
7097 cartesian_trajectory_planner_B.s_j = 0.0;
7098 for (cartesian_trajectory_planner_B.g_idx_0 = 0;
7099 cartesian_trajectory_planner_B.g_idx_0 < 6;
7100 cartesian_trajectory_planner_B.g_idx_0++) {
7101 cartesian_trajectory_planner_B.s_j +=
7102 cartesian_trajectory_planner_B.V[cartesian_trajectory_planner_B.g_idx_0
7103 * 6 + cartesian_trajectory_planner_B.b_i] * AIn->data
7104 [(cartesian_trajectory_planner_B.boffset +
7105 cartesian_trajectory_planner_B.g_idx_0) + 1];
7106 }
7107
7108 unusedU1->data[(cartesian_trajectory_planner_B.coffset +
7109 cartesian_trajectory_planner_B.b_i) + 1] =
7110 cartesian_trajectory_planner_B.s_j;
7111 }
7112 }
7113
7114 for (cartesian_trajectory_planner_B.b_i = 0;
7115 cartesian_trajectory_planner_B.b_i < 6;
7116 cartesian_trajectory_planner_B.b_i++) {
7117 for (cartesian_trajectory_planner_B.idx = 0;
7118 cartesian_trajectory_planner_B.idx < 6;
7119 cartesian_trajectory_planner_B.idx++) {
7120 cartesian_trajectory_planner_B.s_j = 0.0;
7121 for (cartesian_trajectory_planner_B.i_e = 0;
7122 cartesian_trajectory_planner_B.i_e < 6;
7123 cartesian_trajectory_planner_B.i_e++) {
7124 cartesian_trajectory_planner_B.s_j += unusedU1->data[6 *
7125 cartesian_trajectory_planner_B.i_e +
7126 cartesian_trajectory_planner_B.b_i] *
7127 cartesian_trajectory_planner_B.H[6 *
7128 cartesian_trajectory_planner_B.idx +
7129 cartesian_trajectory_planner_B.i_e];
7130 }
7131
7132 cartesian_trajectory_planner_B.idxl = 6 *
7133 cartesian_trajectory_planner_B.idx +
7134 cartesian_trajectory_planner_B.b_i;
7135 cartesian_trajectory_planner_B.H_m[cartesian_trajectory_planner_B.idxl] =
7136 cartesian_trajectory_planner_B.H[cartesian_trajectory_planner_B.idxl]
7137 - cartesian_trajectory_planner_B.s_j;
7138 }
7139 }
7140
7141 memcpy(&cartesian_trajectory_planner_B.H[0],
7142 &cartesian_trajectory_planner_B.H_m[0], 36U * sizeof(real_T));
7143 }
7144
7145 cartesian_trajec_emxFree_real_T(&A_1);
7146 cartesian_trajec_emxFree_real_T(&A_0);
7147 for (cartesian_trajectory_planner_B.i_e = 0;
7148 cartesian_trajectory_planner_B.i_e < 6;
7149 cartesian_trajectory_planner_B.i_e++) {
7150 xSol[cartesian_trajectory_planner_B.i_e] =
7151 cartesian_trajectory_planner_B.x[cartesian_trajectory_planner_B.i_e];
7152 }
7153
7154 cartesian_trajectory_planner_B.A_i = obj->MaxNumIterationInternal;
7155 cartesian_trajectory_planner_B.g_idx_0 = 0;
7156 cartesian_trajec_emxInit_real_T(&L, 1);
7157 cartesian_traje_emxInit_int32_T(&cb, 1);
7158 cartesian_traje_emxInit_int32_T(&db, 1);
7159 cartesian_traje_emxInit_int32_T(&eb, 1);
7160 cartesian_traje_emxInit_int32_T(&fb, 1);
7161 cartesian_traje_emxInit_int32_T(&gb, 1);
7162 cartesian_trajec_emxInit_real_T(&a, 2);
7163 cartesian_trajec_emxInit_real_T(&y, 1);
7164 cartesian_traje_emxInit_int32_T(&ii_0, 1);
7165 cartesian_trajec_emxInit_real_T(&y_0, 2);
7166 cartesian_tra_emxInit_boolean_T(&x, 1);
7167 cartesian_trajec_emxInit_real_T(&A_2, 2);
7168 cartesian_trajec_emxInit_real_T(&sigma, 2);
7169 cartesian_trajec_emxInit_real_T(&tmp, 2);
7170 cartesian_trajec_emxInit_real_T(&tmp_0, 2);
7171 cartesian_trajec_emxInit_real_T(&grad_0, 2);
7172 cartesian_trajec_emxInit_real_T(&sNew, 2);
7173 cartesian_traje_emxInit_int32_T(&ii_1, 1);
7174 cartesian_traje_emxInit_int32_T(&ii_2, 1);
7175 cartesian_trajec_emxInit_real_T(&grad_1, 2);
7176 cartesian_trajec_emxInit_real_T(&A_3, 2);
7177 cartesian_trajec_emxInit_real_T(&alpha_0, 2);
7178 cartesian_traje_emxInit_int32_T(&ii_3, 1);
7179 do {
7180 exitg2 = 0;
7181 if (cartesian_trajectory_planner_B.g_idx_0 <= static_cast<int32_T>
7182 (cartesian_trajectory_planner_B.A_i) - 1) {
7183 cartesian_trajectory_planner_B.s_j = SystemTimeProvider_getElapsedTi
7184 (&obj->TimeObjInternal);
7185 cartesian_trajectory_planner_B.flag = (cartesian_trajectory_planner_B.s_j >
7186 obj->MaxTimeInternal);
7187 if (cartesian_trajectory_planner_B.flag) {
7188 *exitFlag = TimeLimitExceeded;
7189 args = obj->ExtraArgs;
7190 for (cartesian_trajectory_planner_B.b_i = 0;
7191 cartesian_trajectory_planner_B.b_i < 36;
7192 cartesian_trajectory_planner_B.b_i++) {
7193 cartesian_trajectory_planner_B.unusedU0[cartesian_trajectory_planner_B.b_i]
7194 = args->WeightMatrix[cartesian_trajectory_planner_B.b_i];
7195 }
7196
7197 cartesian_trajectory_planner_B.b_i = grad->size[0];
7198 grad->size[0] = args->ErrTemp->size[0];
7199 cartes_emxEnsureCapacity_real_T(grad, cartesian_trajectory_planner_B.b_i);
7200 cartesian_trajectory_planner_B.i_e = args->ErrTemp->size[0];
7201 for (cartesian_trajectory_planner_B.b_i = 0;
7202 cartesian_trajectory_planner_B.b_i <
7203 cartesian_trajectory_planner_B.i_e;
7204 cartesian_trajectory_planner_B.b_i++) {
7205 grad->data[cartesian_trajectory_planner_B.b_i] = args->ErrTemp->
7206 data[cartesian_trajectory_planner_B.b_i];
7207 }
7208
7209 for (cartesian_trajectory_planner_B.b_i = 0;
7210 cartesian_trajectory_planner_B.b_i < 6;
7211 cartesian_trajectory_planner_B.b_i++) {
7212 cartesian_trajectory_planner_B.x[cartesian_trajectory_planner_B.b_i] =
7213 0.0;
7214 for (cartesian_trajectory_planner_B.idx = 0;
7215 cartesian_trajectory_planner_B.idx < 6;
7216 cartesian_trajectory_planner_B.idx++) {
7217 cartesian_trajectory_planner_B.A_i =
7218 cartesian_trajectory_planner_B.unusedU0[6 *
7219 cartesian_trajectory_planner_B.idx +
7220 cartesian_trajectory_planner_B.b_i] * grad->
7221 data[cartesian_trajectory_planner_B.idx] +
7222 cartesian_trajectory_planner_B.x[cartesian_trajectory_planner_B.b_i];
7223 cartesian_trajectory_planner_B.x[cartesian_trajectory_planner_B.b_i]
7224 = cartesian_trajectory_planner_B.A_i;
7225 }
7226 }
7227
7228 *err = cartesian_trajectory_pla_norm_a(cartesian_trajectory_planner_B.x);
7229 *iter = static_cast<real_T>(cartesian_trajectory_planner_B.g_idx_0) +
7230 1.0;
7231 exitg2 = 1;
7232 } else {
7233 if ((A->size[0] == 0) || (A->size[1] == 0)) {
7234 cartesian_trajectory_planner_B.b_i = alpha->size[0];
7235 alpha->size[0] = 1;
7236 cartes_emxEnsureCapacity_real_T(alpha,
7237 cartesian_trajectory_planner_B.b_i);
7238 alpha->data[0] = 0.0;
7239 } else {
7240 cartesian_trajectory_planner_B.m_p = A->size[1] - 1;
7241 cartesian_trajectory_planner_B.inner = A->size[0] - 1;
7242 cartesian_trajectory_planner_B.n_f = A->size[1] - 1;
7243 cartesian_trajectory_planner_B.b_i = AIn->size[0] * AIn->size[1];
7244 AIn->size[0] = A->size[1];
7245 AIn->size[1] = A->size[1];
7246 cartes_emxEnsureCapacity_real_T(AIn,
7247 cartesian_trajectory_planner_B.b_i);
7248 for (cartesian_trajectory_planner_B.idx = 0;
7249 cartesian_trajectory_planner_B.idx <=
7250 cartesian_trajectory_planner_B.n_f;
7251 cartesian_trajectory_planner_B.idx++) {
7252 cartesian_trajectory_planner_B.coffset =
7253 (cartesian_trajectory_planner_B.m_p + 1) *
7254 cartesian_trajectory_planner_B.idx - 1;
7255 cartesian_trajectory_planner_B.boffset =
7256 cartesian_trajectory_planner_B.idx * A->size[0] - 1;
7257 for (cartesian_trajectory_planner_B.b_i = 0;
7258 cartesian_trajectory_planner_B.b_i <=
7259 cartesian_trajectory_planner_B.m_p;
7260 cartesian_trajectory_planner_B.b_i++) {
7261 AIn->data[(cartesian_trajectory_planner_B.coffset +
7262 cartesian_trajectory_planner_B.b_i) + 1] = 0.0;
7263 }
7264
7265 for (cartesian_trajectory_planner_B.nx_i = 0;
7266 cartesian_trajectory_planner_B.nx_i <=
7267 cartesian_trajectory_planner_B.inner;
7268 cartesian_trajectory_planner_B.nx_i++) {
7269 cartesian_trajectory_planner_B.s_j = A->data
7270 [(cartesian_trajectory_planner_B.boffset +
7271 cartesian_trajectory_planner_B.nx_i) + 1];
7272 for (cartesian_trajectory_planner_B.j = 0;
7273 cartesian_trajectory_planner_B.j <=
7274 cartesian_trajectory_planner_B.m_p;
7275 cartesian_trajectory_planner_B.j++) {
7276 cartesian_trajectory_planner_B.b_i =
7277 (cartesian_trajectory_planner_B.coffset +
7278 cartesian_trajectory_planner_B.j) + 1;
7279 AIn->data[cartesian_trajectory_planner_B.b_i] += A->
7280 data[cartesian_trajectory_planner_B.j * A->size[0] +
7281 cartesian_trajectory_planner_B.nx_i] *
7282 cartesian_trajectory_planner_B.s_j;
7283 }
7284 }
7285 }
7286
7287 cartesian_trajectory_planner_B.b_i = A_2->size[0] * A_2->size[1];
7288 A_2->size[0] = A->size[1];
7289 A_2->size[1] = A->size[0];
7290 cartes_emxEnsureCapacity_real_T(A_2,
7291 cartesian_trajectory_planner_B.b_i);
7292 cartesian_trajectory_planner_B.i_e = A->size[0];
7293 for (cartesian_trajectory_planner_B.b_i = 0;
7294 cartesian_trajectory_planner_B.b_i <
7295 cartesian_trajectory_planner_B.i_e;
7296 cartesian_trajectory_planner_B.b_i++) {
7297 cartesian_trajectory_planner_B.n_f = A->size[1];
7298 for (cartesian_trajectory_planner_B.idx = 0;
7299 cartesian_trajectory_planner_B.idx <
7300 cartesian_trajectory_planner_B.n_f;
7301 cartesian_trajectory_planner_B.idx++) {
7302 A_2->data[cartesian_trajectory_planner_B.idx + A_2->size[0] *
7303 cartesian_trajectory_planner_B.b_i] = A->data[A->size[0] *
7304 cartesian_trajectory_planner_B.idx +
7305 cartesian_trajectory_planner_B.b_i];
7306 }
7307 }
7308
7309 cartesian_trajectory_p_mldivide(AIn, A_2, a);
7310 cartesian_trajectory_planner_B.m_p = a->size[0] - 1;
7311 cartesian_trajectory_planner_B.inner = a->size[1] - 1;
7312 cartesian_trajectory_planner_B.b_i = alpha->size[0];
7313 alpha->size[0] = a->size[0];
7314 cartes_emxEnsureCapacity_real_T(alpha,
7315 cartesian_trajectory_planner_B.b_i);
7316 for (cartesian_trajectory_planner_B.b_i = 0;
7317 cartesian_trajectory_planner_B.b_i <=
7318 cartesian_trajectory_planner_B.m_p;
7319 cartesian_trajectory_planner_B.b_i++) {
7320 alpha->data[cartesian_trajectory_planner_B.b_i] = 0.0;
7321 }
7322
7323 for (cartesian_trajectory_planner_B.nx_i = 0;
7324 cartesian_trajectory_planner_B.nx_i <=
7325 cartesian_trajectory_planner_B.inner;
7326 cartesian_trajectory_planner_B.nx_i++) {
7327 cartesian_trajectory_planner_B.aoffset =
7328 cartesian_trajectory_planner_B.nx_i * a->size[0] - 1;
7329 for (cartesian_trajectory_planner_B.j = 0;
7330 cartesian_trajectory_planner_B.j <=
7331 cartesian_trajectory_planner_B.m_p;
7332 cartesian_trajectory_planner_B.j++) {
7333 alpha->data[cartesian_trajectory_planner_B.j] += a->data
7334 [(cartesian_trajectory_planner_B.aoffset +
7335 cartesian_trajectory_planner_B.j) + 1] * grad->
7336 data[cartesian_trajectory_planner_B.nx_i];
7337 }
7338 }
7339 }
7340
7341 for (cartesian_trajectory_planner_B.b_i = 0;
7342 cartesian_trajectory_planner_B.b_i < 6;
7343 cartesian_trajectory_planner_B.b_i++) {
7344 cartesian_trajectory_planner_B.Hg[cartesian_trajectory_planner_B.b_i] =
7345 0.0;
7346 for (cartesian_trajectory_planner_B.idx = 0;
7347 cartesian_trajectory_planner_B.idx < 6;
7348 cartesian_trajectory_planner_B.idx++) {
7349 cartesian_trajectory_planner_B.b_gamma =
7350 cartesian_trajectory_planner_B.H[6 *
7351 cartesian_trajectory_planner_B.idx +
7352 cartesian_trajectory_planner_B.b_i] * grad->
7353 data[cartesian_trajectory_planner_B.idx] +
7354 cartesian_trajectory_planner_B.Hg[cartesian_trajectory_planner_B.b_i];
7355 cartesian_trajectory_planner_B.Hg[cartesian_trajectory_planner_B.b_i]
7356 = cartesian_trajectory_planner_B.b_gamma;
7357 }
7358 }
7359
7360 if (DampedBFGSwGradientProjection_a(obj,
7361 cartesian_trajectory_planner_B.Hg, alpha)) {
7362 *exitFlag = LocalMinimumFound;
7363 args = obj->ExtraArgs;
7364 for (cartesian_trajectory_planner_B.b_i = 0;
7365 cartesian_trajectory_planner_B.b_i < 36;
7366 cartesian_trajectory_planner_B.b_i++) {
7367 cartesian_trajectory_planner_B.unusedU0[cartesian_trajectory_planner_B.b_i]
7368 = args->WeightMatrix[cartesian_trajectory_planner_B.b_i];
7369 }
7370
7371 cartesian_trajectory_planner_B.b_i = grad->size[0];
7372 grad->size[0] = args->ErrTemp->size[0];
7373 cartes_emxEnsureCapacity_real_T(grad,
7374 cartesian_trajectory_planner_B.b_i);
7375 cartesian_trajectory_planner_B.i_e = args->ErrTemp->size[0];
7376 for (cartesian_trajectory_planner_B.b_i = 0;
7377 cartesian_trajectory_planner_B.b_i <
7378 cartesian_trajectory_planner_B.i_e;
7379 cartesian_trajectory_planner_B.b_i++) {
7380 grad->data[cartesian_trajectory_planner_B.b_i] = args->ErrTemp->
7381 data[cartesian_trajectory_planner_B.b_i];
7382 }
7383
7384 for (cartesian_trajectory_planner_B.b_i = 0;
7385 cartesian_trajectory_planner_B.b_i < 6;
7386 cartesian_trajectory_planner_B.b_i++) {
7387 cartesian_trajectory_planner_B.x[cartesian_trajectory_planner_B.b_i]
7388 = 0.0;
7389 for (cartesian_trajectory_planner_B.idx = 0;
7390 cartesian_trajectory_planner_B.idx < 6;
7391 cartesian_trajectory_planner_B.idx++) {
7392 cartesian_trajectory_planner_B.A_i =
7393 cartesian_trajectory_planner_B.unusedU0[6 *
7394 cartesian_trajectory_planner_B.idx +
7395 cartesian_trajectory_planner_B.b_i] * grad->
7396 data[cartesian_trajectory_planner_B.idx] +
7397 cartesian_trajectory_planner_B.x[cartesian_trajectory_planner_B.b_i];
7398 cartesian_trajectory_planner_B.x[cartesian_trajectory_planner_B.b_i]
7399 = cartesian_trajectory_planner_B.A_i;
7400 }
7401 }
7402
7403 *err = cartesian_trajectory_pla_norm_a
7404 (cartesian_trajectory_planner_B.x);
7405 *iter = static_cast<real_T>(cartesian_trajectory_planner_B.g_idx_0) +
7406 1.0;
7407 exitg2 = 1;
7408 } else {
7409 guard1 = false;
7410 guard2 = false;
7411 if (obj->ConstraintsOn && ((A->size[0] != 0) && (A->size[1] != 0))) {
7412 cartesian_trajectory_planner_B.m_p = A->size[1] - 1;
7413 cartesian_trajectory_planner_B.inner = A->size[0] - 1;
7414 cartesian_trajectory_planner_B.n_f = A->size[1] - 1;
7415 cartesian_trajectory_planner_B.b_i = AIn->size[0] * AIn->size[1];
7416 AIn->size[0] = A->size[1];
7417 AIn->size[1] = A->size[1];
7418 cartes_emxEnsureCapacity_real_T(AIn,
7419 cartesian_trajectory_planner_B.b_i);
7420 for (cartesian_trajectory_planner_B.idx = 0;
7421 cartesian_trajectory_planner_B.idx <=
7422 cartesian_trajectory_planner_B.n_f;
7423 cartesian_trajectory_planner_B.idx++) {
7424 cartesian_trajectory_planner_B.coffset =
7425 (cartesian_trajectory_planner_B.m_p + 1) *
7426 cartesian_trajectory_planner_B.idx - 1;
7427 cartesian_trajectory_planner_B.boffset =
7428 cartesian_trajectory_planner_B.idx * A->size[0] - 1;
7429 for (cartesian_trajectory_planner_B.b_i = 0;
7430 cartesian_trajectory_planner_B.b_i <=
7431 cartesian_trajectory_planner_B.m_p;
7432 cartesian_trajectory_planner_B.b_i++) {
7433 AIn->data[(cartesian_trajectory_planner_B.coffset +
7434 cartesian_trajectory_planner_B.b_i) + 1] = 0.0;
7435 }
7436
7437 for (cartesian_trajectory_planner_B.nx_i = 0;
7438 cartesian_trajectory_planner_B.nx_i <=
7439 cartesian_trajectory_planner_B.inner;
7440 cartesian_trajectory_planner_B.nx_i++) {
7441 cartesian_trajectory_planner_B.s_j = A->data
7442 [(cartesian_trajectory_planner_B.boffset +
7443 cartesian_trajectory_planner_B.nx_i) + 1];
7444 for (cartesian_trajectory_planner_B.j = 0;
7445 cartesian_trajectory_planner_B.j <=
7446 cartesian_trajectory_planner_B.m_p;
7447 cartesian_trajectory_planner_B.j++) {
7448 cartesian_trajectory_planner_B.b_i =
7449 (cartesian_trajectory_planner_B.coffset +
7450 cartesian_trajectory_planner_B.j) + 1;
7451 AIn->data[cartesian_trajectory_planner_B.b_i] += A->
7452 data[cartesian_trajectory_planner_B.j * A->size[0] +
7453 cartesian_trajectory_planner_B.nx_i] *
7454 cartesian_trajectory_planner_B.s_j;
7455 }
7456 }
7457 }
7458
7459 cartesian_trajectory_planne_inv(AIn, a);
7460 cartesian_trajectory_plann_diag(a, L);
7461 cartesian_trajectory_pl_sqrt_as(L);
7462 cartesian_trajectory_planner_B.b_i = alpha->size[0];
7463 cartes_emxEnsureCapacity_real_T(alpha,
7464 cartesian_trajectory_planner_B.b_i);
7465 cartesian_trajectory_planner_B.i_e = alpha->size[0];
7466 for (cartesian_trajectory_planner_B.b_i = 0;
7467 cartesian_trajectory_planner_B.b_i <
7468 cartesian_trajectory_planner_B.i_e;
7469 cartesian_trajectory_planner_B.b_i++) {
7470 alpha->data[cartesian_trajectory_planner_B.b_i] /= L->
7471 data[cartesian_trajectory_planner_B.b_i];
7472 }
7473
7474 cartesian_trajectory_planner_B.n_f = alpha->size[0];
7475 if (alpha->size[0] <= 2) {
7476 if (alpha->size[0] == 1) {
7477 cartesian_trajectory_planner_B.s_j = alpha->data[0];
7478 cartesian_trajectory_planner_B.idxl = 0;
7479 } else if ((alpha->data[0] < alpha->data[1]) || (rtIsNaN
7480 (alpha->data[0]) && (!rtIsNaN(alpha->data[1])))) {
7481 cartesian_trajectory_planner_B.s_j = alpha->data[1];
7482 cartesian_trajectory_planner_B.idxl = 1;
7483 } else {
7484 cartesian_trajectory_planner_B.s_j = alpha->data[0];
7485 cartesian_trajectory_planner_B.idxl = 0;
7486 }
7487 } else {
7488 if (!rtIsNaN(alpha->data[0])) {
7489 cartesian_trajectory_planner_B.idxl = 1;
7490 } else {
7491 cartesian_trajectory_planner_B.idxl = 0;
7492 cartesian_trajectory_planner_B.b_i = 2;
7493 exitg3 = false;
7494 while ((!exitg3) && (cartesian_trajectory_planner_B.b_i <=
7495 alpha->size[0])) {
7496 if (!rtIsNaN(alpha->data[cartesian_trajectory_planner_B.b_i -
7497 1])) {
7498 cartesian_trajectory_planner_B.idxl =
7499 cartesian_trajectory_planner_B.b_i;
7500 exitg3 = true;
7501 } else {
7502 cartesian_trajectory_planner_B.b_i++;
7503 }
7504 }
7505 }
7506
7507 if (cartesian_trajectory_planner_B.idxl == 0) {
7508 cartesian_trajectory_planner_B.s_j = alpha->data[0];
7509 } else {
7510 cartesian_trajectory_planner_B.s_j = alpha->
7511 data[cartesian_trajectory_planner_B.idxl - 1];
7512 cartesian_trajectory_planner_B.nx_i =
7513 cartesian_trajectory_planner_B.idxl;
7514 for (cartesian_trajectory_planner_B.b_i =
7515 cartesian_trajectory_planner_B.idxl + 1;
7516 cartesian_trajectory_planner_B.b_i <=
7517 cartesian_trajectory_planner_B.n_f;
7518 cartesian_trajectory_planner_B.b_i++) {
7519 if (cartesian_trajectory_planner_B.s_j < alpha->
7520 data[cartesian_trajectory_planner_B.b_i - 1]) {
7521 cartesian_trajectory_planner_B.s_j = alpha->
7522 data[cartesian_trajectory_planner_B.b_i - 1];
7523 cartesian_trajectory_planner_B.nx_i =
7524 cartesian_trajectory_planner_B.b_i;
7525 }
7526 }
7527
7528 cartesian_trajectory_planner_B.idxl =
7529 cartesian_trajectory_planner_B.nx_i - 1;
7530 }
7531 }
7532
7533 if (cartesian_trajectory_pla_norm_a
7534 (cartesian_trajectory_planner_B.Hg) < 0.5 *
7535 cartesian_trajectory_planner_B.s_j) {
7536 cartesian_trajectory_planner_B.nx_i = activeSet->size[0];
7537 cartesian_trajectory_planner_B.idx = 0;
7538 cartesian_trajectory_planner_B.b_i = ii->size[0];
7539 ii->size[0] = activeSet->size[0];
7540 carte_emxEnsureCapacity_int32_T(ii,
7541 cartesian_trajectory_planner_B.b_i);
7542 cartesian_trajectory_planner_B.b_i = 1;
7543 exitg3 = false;
7544 while ((!exitg3) && (cartesian_trajectory_planner_B.b_i - 1 <=
7545 cartesian_trajectory_planner_B.nx_i - 1)) {
7546 if (activeSet->data[cartesian_trajectory_planner_B.b_i - 1]) {
7547 cartesian_trajectory_planner_B.idx++;
7548 ii->data[cartesian_trajectory_planner_B.idx - 1] =
7549 cartesian_trajectory_planner_B.b_i;
7550 if (cartesian_trajectory_planner_B.idx >=
7551 cartesian_trajectory_planner_B.nx_i) {
7552 exitg3 = true;
7553 } else {
7554 cartesian_trajectory_planner_B.b_i++;
7555 }
7556 } else {
7557 cartesian_trajectory_planner_B.b_i++;
7558 }
7559 }
7560
7561 if (activeSet->size[0] == 1) {
7562 if (cartesian_trajectory_planner_B.idx == 0) {
7563 ii->size[0] = 0;
7564 }
7565 } else {
7566 if (1 > cartesian_trajectory_planner_B.idx) {
7567 cartesian_trajectory_planner_B.idx = 0;
7568 }
7569
7570 cartesian_trajectory_planner_B.b_i = ii_1->size[0];
7571 ii_1->size[0] = cartesian_trajectory_planner_B.idx;
7572 carte_emxEnsureCapacity_int32_T(ii_1,
7573 cartesian_trajectory_planner_B.b_i);
7574 for (cartesian_trajectory_planner_B.b_i = 0;
7575 cartesian_trajectory_planner_B.b_i <
7576 cartesian_trajectory_planner_B.idx;
7577 cartesian_trajectory_planner_B.b_i++) {
7578 ii_1->data[cartesian_trajectory_planner_B.b_i] = ii->
7579 data[cartesian_trajectory_planner_B.b_i];
7580 }
7581
7582 cartesian_trajectory_planner_B.b_i = ii->size[0];
7583 ii->size[0] = ii_1->size[0];
7584 carte_emxEnsureCapacity_int32_T(ii,
7585 cartesian_trajectory_planner_B.b_i);
7586 cartesian_trajectory_planner_B.i_e = ii_1->size[0];
7587 for (cartesian_trajectory_planner_B.b_i = 0;
7588 cartesian_trajectory_planner_B.b_i <
7589 cartesian_trajectory_planner_B.i_e;
7590 cartesian_trajectory_planner_B.b_i++) {
7591 ii->data[cartesian_trajectory_planner_B.b_i] = ii_1->
7592 data[cartesian_trajectory_planner_B.b_i];
7593 }
7594 }
7595
7596 cartesian_trajectory_planner_B.b_i = alpha->size[0];
7597 alpha->size[0] = ii->size[0];
7598 cartes_emxEnsureCapacity_real_T(alpha,
7599 cartesian_trajectory_planner_B.b_i);
7600 cartesian_trajectory_planner_B.i_e = ii->size[0];
7601 for (cartesian_trajectory_planner_B.b_i = 0;
7602 cartesian_trajectory_planner_B.b_i <
7603 cartesian_trajectory_planner_B.i_e;
7604 cartesian_trajectory_planner_B.b_i++) {
7605 alpha->data[cartesian_trajectory_planner_B.b_i] = ii->
7606 data[cartesian_trajectory_planner_B.b_i];
7607 }
7608
7609 activeSet->data[static_cast<int32_T>(alpha->
7610 data[cartesian_trajectory_planner_B.idxl]) - 1] = false;
7611 cartesian_trajectory_planner_B.nx_i = activeSet->size[0] - 1;
7612 cartesian_trajectory_planner_B.idx = 0;
7613 for (cartesian_trajectory_planner_B.b_i = 0;
7614 cartesian_trajectory_planner_B.b_i <=
7615 cartesian_trajectory_planner_B.nx_i;
7616 cartesian_trajectory_planner_B.b_i++) {
7617 if (activeSet->data[cartesian_trajectory_planner_B.b_i]) {
7618 cartesian_trajectory_planner_B.idx++;
7619 }
7620 }
7621
7622 cartesian_trajectory_planner_B.b_i = eb->size[0];
7623 eb->size[0] = cartesian_trajectory_planner_B.idx;
7624 carte_emxEnsureCapacity_int32_T(eb,
7625 cartesian_trajectory_planner_B.b_i);
7626 cartesian_trajectory_planner_B.idx = 0;
7627 for (cartesian_trajectory_planner_B.b_i = 0;
7628 cartesian_trajectory_planner_B.b_i <=
7629 cartesian_trajectory_planner_B.nx_i;
7630 cartesian_trajectory_planner_B.b_i++) {
7631 if (activeSet->data[cartesian_trajectory_planner_B.b_i]) {
7632 eb->data[cartesian_trajectory_planner_B.idx] =
7633 cartesian_trajectory_planner_B.b_i + 1;
7634 cartesian_trajectory_planner_B.idx++;
7635 }
7636 }
7637
7638 cartesian_trajectory_planner_B.i_e = obj->ConstraintMatrix->size[0];
7639 cartesian_trajectory_planner_B.b_i = A->size[0] * A->size[1];
7640 A->size[0] = cartesian_trajectory_planner_B.i_e;
7641 A->size[1] = eb->size[0];
7642 cartes_emxEnsureCapacity_real_T(A,
7643 cartesian_trajectory_planner_B.b_i);
7644 cartesian_trajectory_planner_B.n_f = eb->size[0];
7645 for (cartesian_trajectory_planner_B.b_i = 0;
7646 cartesian_trajectory_planner_B.b_i <
7647 cartesian_trajectory_planner_B.n_f;
7648 cartesian_trajectory_planner_B.b_i++) {
7649 for (cartesian_trajectory_planner_B.idx = 0;
7650 cartesian_trajectory_planner_B.idx <
7651 cartesian_trajectory_planner_B.i_e;
7652 cartesian_trajectory_planner_B.idx++) {
7653 A->data[cartesian_trajectory_planner_B.idx + A->size[0] *
7654 cartesian_trajectory_planner_B.b_i] = obj->
7655 ConstraintMatrix->data[(eb->
7656 data[cartesian_trajectory_planner_B.b_i] - 1) *
7657 obj->ConstraintMatrix->size[0] +
7658 cartesian_trajectory_planner_B.idx];
7659 }
7660 }
7661
7662 cartesian_trajectory_planner_B.m_p = A->size[1] - 1;
7663 cartesian_trajectory_planner_B.inner = A->size[0] - 1;
7664 cartesian_trajectory_planner_B.n_f = A->size[1] - 1;
7665 cartesian_trajectory_planner_B.b_i = AIn->size[0] * AIn->size[1];
7666 AIn->size[0] = A->size[1];
7667 AIn->size[1] = A->size[1];
7668 cartes_emxEnsureCapacity_real_T(AIn,
7669 cartesian_trajectory_planner_B.b_i);
7670 for (cartesian_trajectory_planner_B.idx = 0;
7671 cartesian_trajectory_planner_B.idx <=
7672 cartesian_trajectory_planner_B.n_f;
7673 cartesian_trajectory_planner_B.idx++) {
7674 cartesian_trajectory_planner_B.coffset =
7675 (cartesian_trajectory_planner_B.m_p + 1) *
7676 cartesian_trajectory_planner_B.idx - 1;
7677 cartesian_trajectory_planner_B.boffset =
7678 cartesian_trajectory_planner_B.idx * A->size[0] - 1;
7679 for (cartesian_trajectory_planner_B.b_i = 0;
7680 cartesian_trajectory_planner_B.b_i <=
7681 cartesian_trajectory_planner_B.m_p;
7682 cartesian_trajectory_planner_B.b_i++) {
7683 AIn->data[(cartesian_trajectory_planner_B.coffset +
7684 cartesian_trajectory_planner_B.b_i) + 1] = 0.0;
7685 }
7686
7687 for (cartesian_trajectory_planner_B.nx_i = 0;
7688 cartesian_trajectory_planner_B.nx_i <=
7689 cartesian_trajectory_planner_B.inner;
7690 cartesian_trajectory_planner_B.nx_i++) {
7691 cartesian_trajectory_planner_B.s_j = A->data
7692 [(cartesian_trajectory_planner_B.boffset +
7693 cartesian_trajectory_planner_B.nx_i) + 1];
7694 for (cartesian_trajectory_planner_B.j = 0;
7695 cartesian_trajectory_planner_B.j <=
7696 cartesian_trajectory_planner_B.m_p;
7697 cartesian_trajectory_planner_B.j++) {
7698 cartesian_trajectory_planner_B.b_i =
7699 (cartesian_trajectory_planner_B.coffset +
7700 cartesian_trajectory_planner_B.j) + 1;
7701 AIn->data[cartesian_trajectory_planner_B.b_i] += A->
7702 data[cartesian_trajectory_planner_B.j * A->size[0] +
7703 cartesian_trajectory_planner_B.nx_i] *
7704 cartesian_trajectory_planner_B.s_j;
7705 }
7706 }
7707 }
7708
7709 cartesian_trajectory_planner_B.b_i = A_3->size[0] * A_3->size[1];
7710 A_3->size[0] = A->size[1];
7711 A_3->size[1] = A->size[0];
7712 cartes_emxEnsureCapacity_real_T(A_3,
7713 cartesian_trajectory_planner_B.b_i);
7714 cartesian_trajectory_planner_B.i_e = A->size[0];
7715 for (cartesian_trajectory_planner_B.b_i = 0;
7716 cartesian_trajectory_planner_B.b_i <
7717 cartesian_trajectory_planner_B.i_e;
7718 cartesian_trajectory_planner_B.b_i++) {
7719 cartesian_trajectory_planner_B.n_f = A->size[1];
7720 for (cartesian_trajectory_planner_B.idx = 0;
7721 cartesian_trajectory_planner_B.idx <
7722 cartesian_trajectory_planner_B.n_f;
7723 cartesian_trajectory_planner_B.idx++) {
7724 A_3->data[cartesian_trajectory_planner_B.idx + A_3->size[0] *
7725 cartesian_trajectory_planner_B.b_i] = A->data[A->size[0] *
7726 cartesian_trajectory_planner_B.idx +
7727 cartesian_trajectory_planner_B.b_i];
7728 }
7729 }
7730
7731 cartesian_trajectory_p_mldivide(AIn, A_3, a);
7732 cartesian_trajectory_planner_B.m_p = A->size[0] - 1;
7733 cartesian_trajectory_planner_B.inner = A->size[1] - 1;
7734 cartesian_trajectory_planner_B.n_f = a->size[1] - 1;
7735 cartesian_trajectory_planner_B.b_i = AIn->size[0] * AIn->size[1];
7736 AIn->size[0] = A->size[0];
7737 AIn->size[1] = a->size[1];
7738 cartes_emxEnsureCapacity_real_T(AIn,
7739 cartesian_trajectory_planner_B.b_i);
7740 for (cartesian_trajectory_planner_B.idx = 0;
7741 cartesian_trajectory_planner_B.idx <=
7742 cartesian_trajectory_planner_B.n_f;
7743 cartesian_trajectory_planner_B.idx++) {
7744 cartesian_trajectory_planner_B.coffset =
7745 (cartesian_trajectory_planner_B.m_p + 1) *
7746 cartesian_trajectory_planner_B.idx - 1;
7747 cartesian_trajectory_planner_B.boffset =
7748 cartesian_trajectory_planner_B.idx * a->size[0] - 1;
7749 for (cartesian_trajectory_planner_B.b_i = 0;
7750 cartesian_trajectory_planner_B.b_i <=
7751 cartesian_trajectory_planner_B.m_p;
7752 cartesian_trajectory_planner_B.b_i++) {
7753 AIn->data[(cartesian_trajectory_planner_B.coffset +
7754 cartesian_trajectory_planner_B.b_i) + 1] = 0.0;
7755 }
7756
7757 for (cartesian_trajectory_planner_B.nx_i = 0;
7758 cartesian_trajectory_planner_B.nx_i <=
7759 cartesian_trajectory_planner_B.inner;
7760 cartesian_trajectory_planner_B.nx_i++) {
7761 cartesian_trajectory_planner_B.aoffset =
7762 cartesian_trajectory_planner_B.nx_i * A->size[0] - 1;
7763 cartesian_trajectory_planner_B.s_j = a->data
7764 [(cartesian_trajectory_planner_B.boffset +
7765 cartesian_trajectory_planner_B.nx_i) + 1];
7766 for (cartesian_trajectory_planner_B.j = 0;
7767 cartesian_trajectory_planner_B.j <=
7768 cartesian_trajectory_planner_B.m_p;
7769 cartesian_trajectory_planner_B.j++) {
7770 cartesian_trajectory_planner_B.i_e =
7771 cartesian_trajectory_planner_B.j + 1;
7772 cartesian_trajectory_planner_B.b_i =
7773 cartesian_trajectory_planner_B.coffset +
7774 cartesian_trajectory_planner_B.i_e;
7775 AIn->data[cartesian_trajectory_planner_B.b_i] += A->
7776 data[cartesian_trajectory_planner_B.aoffset +
7777 cartesian_trajectory_planner_B.i_e] *
7778 cartesian_trajectory_planner_B.s_j;
7779 }
7780 }
7781 }
7782
7783 for (cartesian_trajectory_planner_B.b_i = 0;
7784 cartesian_trajectory_planner_B.b_i < 36;
7785 cartesian_trajectory_planner_B.b_i++) {
7786 cartesian_trajectory_planner_B.P[cartesian_trajectory_planner_B.b_i]
7787 =
7788 cartesian_trajectory_planner_B.unusedU0[cartesian_trajectory_planner_B.b_i]
7789 - AIn->data[cartesian_trajectory_planner_B.b_i];
7790 }
7791
7792 cartesian_trajectory_planner_B.s_j = alpha->
7793 data[cartesian_trajectory_planner_B.idxl];
7794 cartesian_trajectory_planner_B.b_i = static_cast<int32_T>
7795 (cartesian_trajectory_planner_B.s_j);
7796 cartesian_trajectory_planner_B.i_e = obj->ConstraintMatrix->size[0];
7797 cartesian_trajectory_planner_B.idx = alpha->size[0];
7798 alpha->size[0] = cartesian_trajectory_planner_B.i_e;
7799 cartes_emxEnsureCapacity_real_T(alpha,
7800 cartesian_trajectory_planner_B.idx);
7801 for (cartesian_trajectory_planner_B.idx = 0;
7802 cartesian_trajectory_planner_B.idx <
7803 cartesian_trajectory_planner_B.i_e;
7804 cartesian_trajectory_planner_B.idx++) {
7805 alpha->data[cartesian_trajectory_planner_B.idx] =
7806 obj->ConstraintMatrix->data
7807 [(cartesian_trajectory_planner_B.b_i - 1) *
7808 obj->ConstraintMatrix->size[0] +
7809 cartesian_trajectory_planner_B.idx];
7810 }
7811
7812 cartesian_trajectory_planner_B.b_i = alpha_0->size[0] *
7813 alpha_0->size[1];
7814 alpha_0->size[0] = 1;
7815 alpha_0->size[1] = alpha->size[0];
7816 cartes_emxEnsureCapacity_real_T(alpha_0,
7817 cartesian_trajectory_planner_B.b_i);
7818 cartesian_trajectory_planner_B.i_e = alpha->size[0];
7819 for (cartesian_trajectory_planner_B.b_i = 0;
7820 cartesian_trajectory_planner_B.b_i <
7821 cartesian_trajectory_planner_B.i_e;
7822 cartesian_trajectory_planner_B.b_i++) {
7823 alpha_0->data[cartesian_trajectory_planner_B.b_i] = alpha->
7824 data[cartesian_trajectory_planner_B.b_i];
7825 }
7826
7827 cartesian_trajectory_planner_B.s_j = 0.0;
7828 for (cartesian_trajectory_planner_B.b_i = 0;
7829 cartesian_trajectory_planner_B.b_i < 6;
7830 cartesian_trajectory_planner_B.b_i++) {
7831 cartesian_trajectory_planner_B.Hg[cartesian_trajectory_planner_B.b_i]
7832 = 0.0;
7833 for (cartesian_trajectory_planner_B.idx = 0;
7834 cartesian_trajectory_planner_B.idx < 6;
7835 cartesian_trajectory_planner_B.idx++) {
7836 cartesian_trajectory_planner_B.b_gamma =
7837 cartesian_trajectory_planner_B.P[6 *
7838 cartesian_trajectory_planner_B.b_i +
7839 cartesian_trajectory_planner_B.idx] * alpha_0->
7840 data[cartesian_trajectory_planner_B.idx] +
7841 cartesian_trajectory_planner_B.Hg[cartesian_trajectory_planner_B.b_i];
7842 cartesian_trajectory_planner_B.Hg[cartesian_trajectory_planner_B.b_i]
7843 = cartesian_trajectory_planner_B.b_gamma;
7844 }
7845
7846 cartesian_trajectory_planner_B.s_j +=
7847 cartesian_trajectory_planner_B.Hg[cartesian_trajectory_planner_B.b_i]
7848 * alpha->data[cartesian_trajectory_planner_B.b_i];
7849 }
7850
7851 cartesian_trajectory_planner_B.s_j = 1.0 /
7852 cartesian_trajectory_planner_B.s_j;
7853 for (cartesian_trajectory_planner_B.b_i = 0;
7854 cartesian_trajectory_planner_B.b_i < 36;
7855 cartesian_trajectory_planner_B.b_i++) {
7856 cartesian_trajectory_planner_B.V[cartesian_trajectory_planner_B.b_i]
7857 = cartesian_trajectory_planner_B.s_j *
7858 cartesian_trajectory_planner_B.P[cartesian_trajectory_planner_B.b_i];
7859 }
7860
7861 cartesian_trajectory_planner_B.b_i = AIn->size[0] * AIn->size[1];
7862 AIn->size[0] = alpha->size[0];
7863 AIn->size[1] = alpha->size[0];
7864 cartes_emxEnsureCapacity_real_T(AIn,
7865 cartesian_trajectory_planner_B.b_i);
7866 cartesian_trajectory_planner_B.i_e = alpha->size[0];
7867 for (cartesian_trajectory_planner_B.b_i = 0;
7868 cartesian_trajectory_planner_B.b_i <
7869 cartesian_trajectory_planner_B.i_e;
7870 cartesian_trajectory_planner_B.b_i++) {
7871 cartesian_trajectory_planner_B.n_f = alpha->size[0];
7872 for (cartesian_trajectory_planner_B.idx = 0;
7873 cartesian_trajectory_planner_B.idx <
7874 cartesian_trajectory_planner_B.n_f;
7875 cartesian_trajectory_planner_B.idx++) {
7876 AIn->data[cartesian_trajectory_planner_B.idx + AIn->size[0] *
7877 cartesian_trajectory_planner_B.b_i] = alpha->
7878 data[cartesian_trajectory_planner_B.idx] * alpha->
7879 data[cartesian_trajectory_planner_B.b_i];
7880 }
7881 }
7882
7883 cartesian_trajectory_planner_B.n_f = AIn->size[1] - 1;
7884 cartesian_trajectory_planner_B.b_i = unusedU1->size[0] *
7885 unusedU1->size[1];
7886 unusedU1->size[0] = 6;
7887 unusedU1->size[1] = AIn->size[1];
7888 cartes_emxEnsureCapacity_real_T(unusedU1,
7889 cartesian_trajectory_planner_B.b_i);
7890 for (cartesian_trajectory_planner_B.idx = 0;
7891 cartesian_trajectory_planner_B.idx <=
7892 cartesian_trajectory_planner_B.n_f;
7893 cartesian_trajectory_planner_B.idx++) {
7894 cartesian_trajectory_planner_B.coffset =
7895 cartesian_trajectory_planner_B.idx * 6 - 1;
7896 cartesian_trajectory_planner_B.boffset =
7897 cartesian_trajectory_planner_B.idx * AIn->size[0] - 1;
7898 for (cartesian_trajectory_planner_B.b_i = 0;
7899 cartesian_trajectory_planner_B.b_i < 6;
7900 cartesian_trajectory_planner_B.b_i++) {
7901 cartesian_trajectory_planner_B.s_j = 0.0;
7902 for (cartesian_trajectory_planner_B.nx_i = 0;
7903 cartesian_trajectory_planner_B.nx_i < 6;
7904 cartesian_trajectory_planner_B.nx_i++) {
7905 cartesian_trajectory_planner_B.s_j +=
7906 cartesian_trajectory_planner_B.V[cartesian_trajectory_planner_B.nx_i
7907 * 6 + cartesian_trajectory_planner_B.b_i] * AIn->data
7908 [(cartesian_trajectory_planner_B.boffset +
7909 cartesian_trajectory_planner_B.nx_i) + 1];
7910 }
7911
7912 unusedU1->data[(cartesian_trajectory_planner_B.coffset +
7913 cartesian_trajectory_planner_B.b_i) + 1] =
7914 cartesian_trajectory_planner_B.s_j;
7915 }
7916 }
7917
7918 for (cartesian_trajectory_planner_B.b_i = 0;
7919 cartesian_trajectory_planner_B.b_i < 6;
7920 cartesian_trajectory_planner_B.b_i++) {
7921 for (cartesian_trajectory_planner_B.idx = 0;
7922 cartesian_trajectory_planner_B.idx < 6;
7923 cartesian_trajectory_planner_B.idx++) {
7924 cartesian_trajectory_planner_B.s_j = 0.0;
7925 for (cartesian_trajectory_planner_B.i_e = 0;
7926 cartesian_trajectory_planner_B.i_e < 6;
7927 cartesian_trajectory_planner_B.i_e++) {
7928 cartesian_trajectory_planner_B.s_j += unusedU1->data[6 *
7929 cartesian_trajectory_planner_B.i_e +
7930 cartesian_trajectory_planner_B.b_i] *
7931 cartesian_trajectory_planner_B.P[6 *
7932 cartesian_trajectory_planner_B.idx +
7933 cartesian_trajectory_planner_B.i_e];
7934 }
7935
7936 cartesian_trajectory_planner_B.idxl = 6 *
7937 cartesian_trajectory_planner_B.idx +
7938 cartesian_trajectory_planner_B.b_i;
7939 cartesian_trajectory_planner_B.H[cartesian_trajectory_planner_B.idxl]
7940 += cartesian_trajectory_planner_B.s_j;
7941 }
7942 }
7943
7944 cartesian_trajectory_planner_B.g_idx_0++;
7945 } else {
7946 guard2 = true;
7947 }
7948 } else {
7949 guard2 = true;
7950 }
7951
7952 if (guard2) {
7953 for (cartesian_trajectory_planner_B.b_i = 0;
7954 cartesian_trajectory_planner_B.b_i < 6;
7955 cartesian_trajectory_planner_B.b_i++) {
7956 cartesian_trajectory_planner_B.Hg[cartesian_trajectory_planner_B.b_i]
7957 =
7958 -cartesian_trajectory_planner_B.Hg[cartesian_trajectory_planner_B.b_i];
7959 }
7960
7961 cartesian_trajectory_planner_B.idxl = -1;
7962 if (obj->ConstraintsOn) {
7963 cartesian_trajectory_planner_B.b_i = x->size[0];
7964 x->size[0] = activeSet->size[0];
7965 car_emxEnsureCapacity_boolean_T(x,
7966 cartesian_trajectory_planner_B.b_i);
7967 cartesian_trajectory_planner_B.i_e = activeSet->size[0];
7968 for (cartesian_trajectory_planner_B.b_i = 0;
7969 cartesian_trajectory_planner_B.b_i <
7970 cartesian_trajectory_planner_B.i_e;
7971 cartesian_trajectory_planner_B.b_i++) {
7972 x->data[cartesian_trajectory_planner_B.b_i] = !activeSet->
7973 data[cartesian_trajectory_planner_B.b_i];
7974 }
7975
7976 if (cartesian_trajectory_planne_any(x)) {
7977 cartesian_trajectory_planner_B.nx_i = activeSet->size[0] - 1;
7978 cartesian_trajectory_planner_B.idx = 0;
7979 for (cartesian_trajectory_planner_B.b_i = 0;
7980 cartesian_trajectory_planner_B.b_i <=
7981 cartesian_trajectory_planner_B.nx_i;
7982 cartesian_trajectory_planner_B.b_i++) {
7983 if (!activeSet->data[cartesian_trajectory_planner_B.b_i]) {
7984 cartesian_trajectory_planner_B.idx++;
7985 }
7986 }
7987
7988 cartesian_trajectory_planner_B.b_i = cb->size[0];
7989 cb->size[0] = cartesian_trajectory_planner_B.idx;
7990 carte_emxEnsureCapacity_int32_T(cb,
7991 cartesian_trajectory_planner_B.b_i);
7992 cartesian_trajectory_planner_B.idx = 0;
7993 for (cartesian_trajectory_planner_B.b_i = 0;
7994 cartesian_trajectory_planner_B.b_i <=
7995 cartesian_trajectory_planner_B.nx_i;
7996 cartesian_trajectory_planner_B.b_i++) {
7997 if (!activeSet->data[cartesian_trajectory_planner_B.b_i]) {
7998 cb->data[cartesian_trajectory_planner_B.idx] =
7999 cartesian_trajectory_planner_B.b_i + 1;
8000 cartesian_trajectory_planner_B.idx++;
8001 }
8002 }
8003
8004 cartesian_trajectory_planner_B.b_i = alpha->size[0];
8005 alpha->size[0] = cb->size[0];
8006 cartes_emxEnsureCapacity_real_T(alpha,
8007 cartesian_trajectory_planner_B.b_i);
8008 cartesian_trajectory_planner_B.i_e = cb->size[0];
8009 for (cartesian_trajectory_planner_B.b_i = 0;
8010 cartesian_trajectory_planner_B.b_i <
8011 cartesian_trajectory_planner_B.i_e;
8012 cartesian_trajectory_planner_B.b_i++) {
8013 alpha->data[cartesian_trajectory_planner_B.b_i] =
8014 obj->ConstraintBound->data[cb->
8015 data[cartesian_trajectory_planner_B.b_i] - 1];
8016 }
8017
8018 cartesian_trajectory_planner_B.nx_i = activeSet->size[0] - 1;
8019 cartesian_trajectory_planner_B.idx = 0;
8020 for (cartesian_trajectory_planner_B.b_i = 0;
8021 cartesian_trajectory_planner_B.b_i <=
8022 cartesian_trajectory_planner_B.nx_i;
8023 cartesian_trajectory_planner_B.b_i++) {
8024 if (!activeSet->data[cartesian_trajectory_planner_B.b_i]) {
8025 cartesian_trajectory_planner_B.idx++;
8026 }
8027 }
8028
8029 cartesian_trajectory_planner_B.b_i = db->size[0];
8030 db->size[0] = cartesian_trajectory_planner_B.idx;
8031 carte_emxEnsureCapacity_int32_T(db,
8032 cartesian_trajectory_planner_B.b_i);
8033 cartesian_trajectory_planner_B.idx = 0;
8034 for (cartesian_trajectory_planner_B.b_i = 0;
8035 cartesian_trajectory_planner_B.b_i <=
8036 cartesian_trajectory_planner_B.nx_i;
8037 cartesian_trajectory_planner_B.b_i++) {
8038 if (!activeSet->data[cartesian_trajectory_planner_B.b_i]) {
8039 db->data[cartesian_trajectory_planner_B.idx] =
8040 cartesian_trajectory_planner_B.b_i + 1;
8041 cartesian_trajectory_planner_B.idx++;
8042 }
8043 }
8044
8045 cartesian_trajectory_planner_B.i_e = obj->ConstraintMatrix->
8046 size[0];
8047 cartesian_trajectory_planner_B.b_i = AIn->size[0] * AIn->size[1];
8048 AIn->size[0] = cartesian_trajectory_planner_B.i_e;
8049 AIn->size[1] = db->size[0];
8050 cartes_emxEnsureCapacity_real_T(AIn,
8051 cartesian_trajectory_planner_B.b_i);
8052 cartesian_trajectory_planner_B.n_f = db->size[0];
8053 for (cartesian_trajectory_planner_B.b_i = 0;
8054 cartesian_trajectory_planner_B.b_i <
8055 cartesian_trajectory_planner_B.n_f;
8056 cartesian_trajectory_planner_B.b_i++) {
8057 for (cartesian_trajectory_planner_B.idx = 0;
8058 cartesian_trajectory_planner_B.idx <
8059 cartesian_trajectory_planner_B.i_e;
8060 cartesian_trajectory_planner_B.idx++) {
8061 AIn->data[cartesian_trajectory_planner_B.idx + AIn->size[0] *
8062 cartesian_trajectory_planner_B.b_i] =
8063 obj->ConstraintMatrix->data[(db->
8064 data[cartesian_trajectory_planner_B.b_i] - 1) *
8065 obj->ConstraintMatrix->size[0] +
8066 cartesian_trajectory_planner_B.idx];
8067 }
8068 }
8069
8070 cartesian_trajectory_planner_B.nx_i = x->size[0];
8071 cartesian_trajectory_planner_B.idx = 0;
8072 cartesian_trajectory_planner_B.b_i = ii->size[0];
8073 ii->size[0] = x->size[0];
8074 carte_emxEnsureCapacity_int32_T(ii,
8075 cartesian_trajectory_planner_B.b_i);
8076 cartesian_trajectory_planner_B.b_i = 1;
8077 exitg3 = false;
8078 while ((!exitg3) && (cartesian_trajectory_planner_B.b_i - 1 <=
8079 cartesian_trajectory_planner_B.nx_i - 1)) {
8080 if (x->data[cartesian_trajectory_planner_B.b_i - 1]) {
8081 cartesian_trajectory_planner_B.idx++;
8082 ii->data[cartesian_trajectory_planner_B.idx - 1] =
8083 cartesian_trajectory_planner_B.b_i;
8084 if (cartesian_trajectory_planner_B.idx >=
8085 cartesian_trajectory_planner_B.nx_i) {
8086 exitg3 = true;
8087 } else {
8088 cartesian_trajectory_planner_B.b_i++;
8089 }
8090 } else {
8091 cartesian_trajectory_planner_B.b_i++;
8092 }
8093 }
8094
8095 if (x->size[0] == 1) {
8096 if (cartesian_trajectory_planner_B.idx == 0) {
8097 ii->size[0] = 0;
8098 }
8099 } else {
8100 if (1 > cartesian_trajectory_planner_B.idx) {
8101 cartesian_trajectory_planner_B.idx = 0;
8102 }
8103
8104 cartesian_trajectory_planner_B.b_i = ii_2->size[0];
8105 ii_2->size[0] = cartesian_trajectory_planner_B.idx;
8106 carte_emxEnsureCapacity_int32_T(ii_2,
8107 cartesian_trajectory_planner_B.b_i);
8108 for (cartesian_trajectory_planner_B.b_i = 0;
8109 cartesian_trajectory_planner_B.b_i <
8110 cartesian_trajectory_planner_B.idx;
8111 cartesian_trajectory_planner_B.b_i++) {
8112 ii_2->data[cartesian_trajectory_planner_B.b_i] = ii->
8113 data[cartesian_trajectory_planner_B.b_i];
8114 }
8115
8116 cartesian_trajectory_planner_B.b_i = ii->size[0];
8117 ii->size[0] = ii_2->size[0];
8118 carte_emxEnsureCapacity_int32_T(ii,
8119 cartesian_trajectory_planner_B.b_i);
8120 cartesian_trajectory_planner_B.i_e = ii_2->size[0];
8121 for (cartesian_trajectory_planner_B.b_i = 0;
8122 cartesian_trajectory_planner_B.b_i <
8123 cartesian_trajectory_planner_B.i_e;
8124 cartesian_trajectory_planner_B.b_i++) {
8125 ii->data[cartesian_trajectory_planner_B.b_i] = ii_2->
8126 data[cartesian_trajectory_planner_B.b_i];
8127 }
8128 }
8129
8130 cartesian_trajectory_planner_B.m_p = AIn->size[1] - 1;
8131 cartesian_trajectory_planner_B.inner = AIn->size[0] - 1;
8132 cartesian_trajectory_planner_B.b_i = L->size[0];
8133 L->size[0] = AIn->size[1];
8134 cartes_emxEnsureCapacity_real_T(L,
8135 cartesian_trajectory_planner_B.b_i);
8136 for (cartesian_trajectory_planner_B.b_i = 0;
8137 cartesian_trajectory_planner_B.b_i <=
8138 cartesian_trajectory_planner_B.m_p;
8139 cartesian_trajectory_planner_B.b_i++) {
8140 L->data[cartesian_trajectory_planner_B.b_i] = 0.0;
8141 }
8142
8143 for (cartesian_trajectory_planner_B.nx_i = 0;
8144 cartesian_trajectory_planner_B.nx_i <=
8145 cartesian_trajectory_planner_B.inner;
8146 cartesian_trajectory_planner_B.nx_i++) {
8147 for (cartesian_trajectory_planner_B.j = 0;
8148 cartesian_trajectory_planner_B.j <=
8149 cartesian_trajectory_planner_B.m_p;
8150 cartesian_trajectory_planner_B.j++) {
8151 L->data[cartesian_trajectory_planner_B.j] += AIn->
8152 data[cartesian_trajectory_planner_B.j * AIn->size[0] +
8153 cartesian_trajectory_planner_B.nx_i] *
8154 cartesian_trajectory_planner_B.x[cartesian_trajectory_planner_B.nx_i];
8155 }
8156 }
8157
8158 cartesian_trajectory_planner_B.m_p = AIn->size[1] - 1;
8159 cartesian_trajectory_planner_B.inner = AIn->size[0] - 1;
8160 cartesian_trajectory_planner_B.b_i = y->size[0];
8161 y->size[0] = AIn->size[1];
8162 cartes_emxEnsureCapacity_real_T(y,
8163 cartesian_trajectory_planner_B.b_i);
8164 for (cartesian_trajectory_planner_B.b_i = 0;
8165 cartesian_trajectory_planner_B.b_i <=
8166 cartesian_trajectory_planner_B.m_p;
8167 cartesian_trajectory_planner_B.b_i++) {
8168 y->data[cartesian_trajectory_planner_B.b_i] = 0.0;
8169 }
8170
8171 for (cartesian_trajectory_planner_B.nx_i = 0;
8172 cartesian_trajectory_planner_B.nx_i <=
8173 cartesian_trajectory_planner_B.inner;
8174 cartesian_trajectory_planner_B.nx_i++) {
8175 for (cartesian_trajectory_planner_B.j = 0;
8176 cartesian_trajectory_planner_B.j <=
8177 cartesian_trajectory_planner_B.m_p;
8178 cartesian_trajectory_planner_B.j++) {
8179 y->data[cartesian_trajectory_planner_B.j] += AIn->
8180 data[cartesian_trajectory_planner_B.j * AIn->size[0] +
8181 cartesian_trajectory_planner_B.nx_i] *
8182 cartesian_trajectory_planner_B.Hg[cartesian_trajectory_planner_B.nx_i];
8183 }
8184 }
8185
8186 cartesian_trajectory_planner_B.b_i = alpha->size[0];
8187 cartes_emxEnsureCapacity_real_T(alpha,
8188 cartesian_trajectory_planner_B.b_i);
8189 cartesian_trajectory_planner_B.i_e = alpha->size[0];
8190 for (cartesian_trajectory_planner_B.b_i = 0;
8191 cartesian_trajectory_planner_B.b_i <
8192 cartesian_trajectory_planner_B.i_e;
8193 cartesian_trajectory_planner_B.b_i++) {
8194 alpha->data[cartesian_trajectory_planner_B.b_i] = (alpha->
8195 data[cartesian_trajectory_planner_B.b_i] - L->
8196 data[cartesian_trajectory_planner_B.b_i]) / y->
8197 data[cartesian_trajectory_planner_B.b_i];
8198 }
8199
8200 cartesian_trajectory_planner_B.b_i = x->size[0];
8201 x->size[0] = alpha->size[0];
8202 car_emxEnsureCapacity_boolean_T(x,
8203 cartesian_trajectory_planner_B.b_i);
8204 cartesian_trajectory_planner_B.i_e = alpha->size[0];
8205 for (cartesian_trajectory_planner_B.b_i = 0;
8206 cartesian_trajectory_planner_B.b_i <
8207 cartesian_trajectory_planner_B.i_e;
8208 cartesian_trajectory_planner_B.b_i++) {
8209 x->data[cartesian_trajectory_planner_B.b_i] = (alpha->
8210 data[cartesian_trajectory_planner_B.b_i] > 0.0);
8211 }
8212
8213 cartesian_trajectory_planner_B.nx_i = x->size[0];
8214 cartesian_trajectory_planner_B.idx = 0;
8215 cartesian_trajectory_planner_B.b_i = ii_0->size[0];
8216 ii_0->size[0] = x->size[0];
8217 carte_emxEnsureCapacity_int32_T(ii_0,
8218 cartesian_trajectory_planner_B.b_i);
8219 cartesian_trajectory_planner_B.b_i = 1;
8220 exitg3 = false;
8221 while ((!exitg3) && (cartesian_trajectory_planner_B.b_i - 1 <=
8222 cartesian_trajectory_planner_B.nx_i - 1)) {
8223 if (x->data[cartesian_trajectory_planner_B.b_i - 1]) {
8224 cartesian_trajectory_planner_B.idx++;
8225 ii_0->data[cartesian_trajectory_planner_B.idx - 1] =
8226 cartesian_trajectory_planner_B.b_i;
8227 if (cartesian_trajectory_planner_B.idx >=
8228 cartesian_trajectory_planner_B.nx_i) {
8229 exitg3 = true;
8230 } else {
8231 cartesian_trajectory_planner_B.b_i++;
8232 }
8233 } else {
8234 cartesian_trajectory_planner_B.b_i++;
8235 }
8236 }
8237
8238 if (x->size[0] == 1) {
8239 if (cartesian_trajectory_planner_B.idx == 0) {
8240 ii_0->size[0] = 0;
8241 }
8242 } else {
8243 if (1 > cartesian_trajectory_planner_B.idx) {
8244 cartesian_trajectory_planner_B.idx = 0;
8245 }
8246
8247 cartesian_trajectory_planner_B.b_i = ii_3->size[0];
8248 ii_3->size[0] = cartesian_trajectory_planner_B.idx;
8249 carte_emxEnsureCapacity_int32_T(ii_3,
8250 cartesian_trajectory_planner_B.b_i);
8251 for (cartesian_trajectory_planner_B.b_i = 0;
8252 cartesian_trajectory_planner_B.b_i <
8253 cartesian_trajectory_planner_B.idx;
8254 cartesian_trajectory_planner_B.b_i++) {
8255 ii_3->data[cartesian_trajectory_planner_B.b_i] = ii_0->
8256 data[cartesian_trajectory_planner_B.b_i];
8257 }
8258
8259 cartesian_trajectory_planner_B.b_i = ii_0->size[0];
8260 ii_0->size[0] = ii_3->size[0];
8261 carte_emxEnsureCapacity_int32_T(ii_0,
8262 cartesian_trajectory_planner_B.b_i);
8263 cartesian_trajectory_planner_B.i_e = ii_3->size[0];
8264 for (cartesian_trajectory_planner_B.b_i = 0;
8265 cartesian_trajectory_planner_B.b_i <
8266 cartesian_trajectory_planner_B.i_e;
8267 cartesian_trajectory_planner_B.b_i++) {
8268 ii_0->data[cartesian_trajectory_planner_B.b_i] = ii_3->
8269 data[cartesian_trajectory_planner_B.b_i];
8270 }
8271 }
8272
8273 cartesian_trajectory_planner_B.b_i = L->size[0];
8274 L->size[0] = ii_0->size[0];
8275 cartes_emxEnsureCapacity_real_T(L,
8276 cartesian_trajectory_planner_B.b_i);
8277 cartesian_trajectory_planner_B.i_e = ii_0->size[0];
8278 for (cartesian_trajectory_planner_B.b_i = 0;
8279 cartesian_trajectory_planner_B.b_i <
8280 cartesian_trajectory_planner_B.i_e;
8281 cartesian_trajectory_planner_B.b_i++) {
8282 L->data[cartesian_trajectory_planner_B.b_i] = ii_0->
8283 data[cartesian_trajectory_planner_B.b_i];
8284 }
8285
8286 if (L->size[0] != 0) {
8287 cartesian_trajectory_planner_B.nx_i = alpha->size[0] - 1;
8288 cartesian_trajectory_planner_B.idx = 0;
8289 for (cartesian_trajectory_planner_B.b_i = 0;
8290 cartesian_trajectory_planner_B.b_i <=
8291 cartesian_trajectory_planner_B.nx_i;
8292 cartesian_trajectory_planner_B.b_i++) {
8293 if (alpha->data[cartesian_trajectory_planner_B.b_i] > 0.0) {
8294 cartesian_trajectory_planner_B.idx++;
8295 }
8296 }
8297
8298 cartesian_trajectory_planner_B.b_i = fb->size[0];
8299 fb->size[0] = cartesian_trajectory_planner_B.idx;
8300 carte_emxEnsureCapacity_int32_T(fb,
8301 cartesian_trajectory_planner_B.b_i);
8302 cartesian_trajectory_planner_B.idx = 0;
8303 for (cartesian_trajectory_planner_B.b_i = 0;
8304 cartesian_trajectory_planner_B.b_i <=
8305 cartesian_trajectory_planner_B.nx_i;
8306 cartesian_trajectory_planner_B.b_i++) {
8307 if (alpha->data[cartesian_trajectory_planner_B.b_i] > 0.0) {
8308 fb->data[cartesian_trajectory_planner_B.idx] =
8309 cartesian_trajectory_planner_B.b_i + 1;
8310 cartesian_trajectory_planner_B.idx++;
8311 }
8312 }
8313
8314 cartesian_trajectory_planner_B.n_f = fb->size[0];
8315 if (fb->size[0] <= 2) {
8316 if (fb->size[0] == 1) {
8317 cartesian_trajectory_planner_B.s_j = alpha->data[fb->data
8318 [0] - 1];
8319 cartesian_trajectory_planner_B.idxl = 0;
8320 } else if ((alpha->data[fb->data[0] - 1] > alpha->data
8321 [fb->data[1] - 1]) || (rtIsNaN(alpha->data
8322 [fb->data[0] - 1]) && (!rtIsNaN(alpha->data
8323 [fb->data[1] - 1])))) {
8324 cartesian_trajectory_planner_B.s_j = alpha->data[fb->data
8325 [1] - 1];
8326 cartesian_trajectory_planner_B.idxl = 1;
8327 } else {
8328 cartesian_trajectory_planner_B.s_j = alpha->data[fb->data
8329 [0] - 1];
8330 cartesian_trajectory_planner_B.idxl = 0;
8331 }
8332 } else {
8333 if (!rtIsNaN(alpha->data[fb->data[0] - 1])) {
8334 cartesian_trajectory_planner_B.idxl = 1;
8335 } else {
8336 cartesian_trajectory_planner_B.idxl = 0;
8337 cartesian_trajectory_planner_B.b_i = 2;
8338 exitg3 = false;
8339 while ((!exitg3) && (cartesian_trajectory_planner_B.b_i <=
8340 fb->size[0])) {
8341 if (!rtIsNaN(alpha->data[fb->
8342 data[cartesian_trajectory_planner_B.b_i - 1]
8343 - 1])) {
8344 cartesian_trajectory_planner_B.idxl =
8345 cartesian_trajectory_planner_B.b_i;
8346 exitg3 = true;
8347 } else {
8348 cartesian_trajectory_planner_B.b_i++;
8349 }
8350 }
8351 }
8352
8353 if (cartesian_trajectory_planner_B.idxl == 0) {
8354 cartesian_trajectory_planner_B.s_j = alpha->data[fb->data
8355 [0] - 1];
8356 } else {
8357 cartesian_trajectory_planner_B.s_j = alpha->data[fb->
8358 data[cartesian_trajectory_planner_B.idxl - 1] - 1];
8359 cartesian_trajectory_planner_B.nx_i =
8360 cartesian_trajectory_planner_B.idxl;
8361 for (cartesian_trajectory_planner_B.b_i =
8362 cartesian_trajectory_planner_B.idxl + 1;
8363 cartesian_trajectory_planner_B.b_i <=
8364 cartesian_trajectory_planner_B.n_f;
8365 cartesian_trajectory_planner_B.b_i++) {
8366 if (cartesian_trajectory_planner_B.s_j > alpha->data
8367 [fb->data[cartesian_trajectory_planner_B.b_i - 1] -
8368 1]) {
8369 cartesian_trajectory_planner_B.s_j = alpha->data
8370 [fb->data[cartesian_trajectory_planner_B.b_i - 1] -
8371 1];
8372 cartesian_trajectory_planner_B.nx_i =
8373 cartesian_trajectory_planner_B.b_i;
8374 }
8375 }
8376
8377 cartesian_trajectory_planner_B.idxl =
8378 cartesian_trajectory_planner_B.nx_i - 1;
8379 }
8380 }
8381
8382 cartesian_trajectory_planner_B.idxl = ii->data
8383 [static_cast<int32_T>(L->
8384 data[cartesian_trajectory_planner_B.idxl]) - 1];
8385 } else {
8386 cartesian_trajectory_planner_B.s_j = 0.0;
8387 }
8388 } else {
8389 cartesian_trajectory_planner_B.s_j = 0.0;
8390 }
8391 } else {
8392 cartesian_trajectory_planner_B.s_j = 0.0;
8393 }
8394
8395 if (cartesian_trajectory_planner_B.s_j > 0.0) {
8396 if (1.0 < cartesian_trajectory_planner_B.s_j) {
8397 cartesian_trajectory_planner_B.b_gamma = 1.0;
8398 } else {
8399 cartesian_trajectory_planner_B.b_gamma =
8400 cartesian_trajectory_planner_B.s_j;
8401 }
8402 } else {
8403 cartesian_trajectory_planner_B.b_gamma = 1.0;
8404 }
8405
8406 cartesian_trajectory_planner_B.beta = obj->ArmijoRuleBeta;
8407 cartesian_trajectory_planner_B.sigma = obj->ArmijoRuleSigma;
8408 for (cartesian_trajectory_planner_B.b_i = 0;
8409 cartesian_trajectory_planner_B.b_i < 6;
8410 cartesian_trajectory_planner_B.b_i++) {
8411 cartesian_trajectory_planner_B.sNew_g[cartesian_trajectory_planner_B.b_i]
8412 = cartesian_trajectory_planner_B.b_gamma *
8413 cartesian_trajectory_planner_B.Hg[cartesian_trajectory_planner_B.b_i]
8414 + cartesian_trajectory_planner_B.x[cartesian_trajectory_planner_B.b_i];
8415 }
8416
8417 cartesian_IKHelpers_computeCost
8418 (cartesian_trajectory_planner_B.sNew_g, obj->ExtraArgs,
8419 &cartesian_trajectory_planner_B.costNew,
8420 cartesian_trajectory_planner_B.V, unusedU1, &c);
8421 obj->ExtraArgs = c;
8422 cartesian_trajectory_planner_B.m = 0.0;
8423 do {
8424 exitg1 = 0;
8425 for (cartesian_trajectory_planner_B.i_e = 0;
8426 cartesian_trajectory_planner_B.i_e < 6;
8427 cartesian_trajectory_planner_B.i_e++) {
8428 xSol[cartesian_trajectory_planner_B.i_e] =
8429 cartesian_trajectory_planner_B.b_gamma *
8430 cartesian_trajectory_planner_B.Hg[cartesian_trajectory_planner_B.i_e];
8431 }
8432
8433 cartesian_trajectory_planner_B.b_i = sigma->size[0] * sigma->size
8434 [1];
8435 sigma->size[0] = 1;
8436 sigma->size[1] = grad->size[0];
8437 cartes_emxEnsureCapacity_real_T(sigma,
8438 cartesian_trajectory_planner_B.b_i);
8439 cartesian_trajectory_planner_B.i_e = grad->size[0];
8440 for (cartesian_trajectory_planner_B.b_i = 0;
8441 cartesian_trajectory_planner_B.b_i <
8442 cartesian_trajectory_planner_B.i_e;
8443 cartesian_trajectory_planner_B.b_i++) {
8444 sigma->data[cartesian_trajectory_planner_B.b_i] =
8445 -cartesian_trajectory_planner_B.sigma * grad->
8446 data[cartesian_trajectory_planner_B.b_i];
8447 }
8448
8449 cartesian_trajectory_planner_B.sigma_h = 0.0;
8450 for (cartesian_trajectory_planner_B.b_i = 0;
8451 cartesian_trajectory_planner_B.b_i < 6;
8452 cartesian_trajectory_planner_B.b_i++) {
8453 cartesian_trajectory_planner_B.sigma_h += sigma->
8454 data[cartesian_trajectory_planner_B.b_i] *
8455 xSol[cartesian_trajectory_planner_B.b_i];
8456 }
8457
8458 if (cartesian_trajectory_planner_B.cost -
8459 cartesian_trajectory_planner_B.costNew <
8460 cartesian_trajectory_planner_B.sigma_h) {
8461 cartesian_trajectory_planner_B.flag =
8462 (cartesian_trajectory_planner_B.b_gamma < obj->StepTolerance);
8463 if (cartesian_trajectory_planner_B.flag) {
8464 for (cartesian_trajectory_planner_B.i_e = 0;
8465 cartesian_trajectory_planner_B.i_e < 6;
8466 cartesian_trajectory_planner_B.i_e++) {
8467 xSol[cartesian_trajectory_planner_B.i_e] =
8468 cartesian_trajectory_planner_B.x[cartesian_trajectory_planner_B.i_e];
8469 }
8470
8471 *exitFlag = StepSizeBelowMinimum;
8472 args = obj->ExtraArgs;
8473 for (cartesian_trajectory_planner_B.b_i = 0;
8474 cartesian_trajectory_planner_B.b_i < 36;
8475 cartesian_trajectory_planner_B.b_i++) {
8476 cartesian_trajectory_planner_B.unusedU0[cartesian_trajectory_planner_B.b_i]
8477 = args->WeightMatrix[cartesian_trajectory_planner_B.b_i];
8478 }
8479
8480 cartesian_trajectory_planner_B.b_i = grad->size[0];
8481 grad->size[0] = args->ErrTemp->size[0];
8482 cartes_emxEnsureCapacity_real_T(grad,
8483 cartesian_trajectory_planner_B.b_i);
8484 cartesian_trajectory_planner_B.i_e = args->ErrTemp->size[0];
8485 for (cartesian_trajectory_planner_B.b_i = 0;
8486 cartesian_trajectory_planner_B.b_i <
8487 cartesian_trajectory_planner_B.i_e;
8488 cartesian_trajectory_planner_B.b_i++) {
8489 grad->data[cartesian_trajectory_planner_B.b_i] =
8490 args->ErrTemp->data[cartesian_trajectory_planner_B.b_i];
8491 }
8492
8493 for (cartesian_trajectory_planner_B.b_i = 0;
8494 cartesian_trajectory_planner_B.b_i < 6;
8495 cartesian_trajectory_planner_B.b_i++) {
8496 cartesian_trajectory_planner_B.x[cartesian_trajectory_planner_B.b_i]
8497 = 0.0;
8498 for (cartesian_trajectory_planner_B.idx = 0;
8499 cartesian_trajectory_planner_B.idx < 6;
8500 cartesian_trajectory_planner_B.idx++) {
8501 cartesian_trajectory_planner_B.A_i =
8502 cartesian_trajectory_planner_B.unusedU0[6 *
8503 cartesian_trajectory_planner_B.idx +
8504 cartesian_trajectory_planner_B.b_i] * grad->
8505 data[cartesian_trajectory_planner_B.idx] +
8506 cartesian_trajectory_planner_B.x[cartesian_trajectory_planner_B.b_i];
8507 cartesian_trajectory_planner_B.x[cartesian_trajectory_planner_B.b_i]
8508 = cartesian_trajectory_planner_B.A_i;
8509 }
8510 }
8511
8512 *err = cartesian_trajectory_pla_norm_a
8513 (cartesian_trajectory_planner_B.x);
8514 *iter = static_cast<real_T>
8515 (cartesian_trajectory_planner_B.g_idx_0) + 1.0;
8516 exitg1 = 1;
8517 } else {
8518 cartesian_trajectory_planner_B.b_gamma *=
8519 cartesian_trajectory_planner_B.beta;
8520 cartesian_trajectory_planner_B.m++;
8521 for (cartesian_trajectory_planner_B.b_i = 0;
8522 cartesian_trajectory_planner_B.b_i < 6;
8523 cartesian_trajectory_planner_B.b_i++) {
8524 cartesian_trajectory_planner_B.sNew_g[cartesian_trajectory_planner_B.b_i]
8525 = cartesian_trajectory_planner_B.b_gamma *
8526 cartesian_trajectory_planner_B.Hg[cartesian_trajectory_planner_B.b_i]
8527 + cartesian_trajectory_planner_B.x[cartesian_trajectory_planner_B.b_i];
8528 }
8529
8530 cartesian_IKHelpers_computeCost
8531 (cartesian_trajectory_planner_B.sNew_g, obj->ExtraArgs,
8532 &cartesian_trajectory_planner_B.costNew,
8533 cartesian_trajectory_planner_B.V, unusedU1, &d);
8534 obj->ExtraArgs = d;
8535 }
8536 } else {
8537 for (cartesian_trajectory_planner_B.b_i = 0;
8538 cartesian_trajectory_planner_B.b_i < 6;
8539 cartesian_trajectory_planner_B.b_i++) {
8540 xSol[cartesian_trajectory_planner_B.b_i] +=
8541 cartesian_trajectory_planner_B.x[cartesian_trajectory_planner_B.b_i];
8542 }
8543
8544 args = obj->ExtraArgs;
8545 cartesian_trajectory_planner_B.b_i = alpha->size[0];
8546 alpha->size[0] = args->GradTemp->size[0];
8547 cartes_emxEnsureCapacity_real_T(alpha,
8548 cartesian_trajectory_planner_B.b_i);
8549 cartesian_trajectory_planner_B.i_e = args->GradTemp->size[0];
8550 for (cartesian_trajectory_planner_B.b_i = 0;
8551 cartesian_trajectory_planner_B.b_i <
8552 cartesian_trajectory_planner_B.i_e;
8553 cartesian_trajectory_planner_B.b_i++) {
8554 alpha->data[cartesian_trajectory_planner_B.b_i] =
8555 args->GradTemp->data[cartesian_trajectory_planner_B.b_i];
8556 }
8557
8558 exitg1 = 2;
8559 }
8560 } while (exitg1 == 0);
8561
8562 if (exitg1 == 1) {
8563 exitg2 = 1;
8564 } else if ((cartesian_trajectory_planner_B.m == 0.0) && (fabs
8565 (cartesian_trajectory_planner_B.b_gamma -
8566 cartesian_trajectory_planner_B.s_j) <
8567 1.4901161193847656E-8)) {
8568 cartesian_trajectory_planner_B.i_e = obj->ConstraintMatrix->size[0];
8569 cartesian_trajectory_planner_B.b_i = grad->size[0];
8570 grad->size[0] = cartesian_trajectory_planner_B.i_e;
8571 cartes_emxEnsureCapacity_real_T(grad,
8572 cartesian_trajectory_planner_B.b_i);
8573 for (cartesian_trajectory_planner_B.b_i = 0;
8574 cartesian_trajectory_planner_B.b_i <
8575 cartesian_trajectory_planner_B.i_e;
8576 cartesian_trajectory_planner_B.b_i++) {
8577 grad->data[cartesian_trajectory_planner_B.b_i] =
8578 obj->ConstraintMatrix->data
8579 [(cartesian_trajectory_planner_B.idxl - 1) *
8580 obj->ConstraintMatrix->size[0] +
8581 cartesian_trajectory_planner_B.b_i];
8582 }
8583
8584 activeSet->data[cartesian_trajectory_planner_B.idxl - 1] = true;
8585 cartesian_trajectory_planner_B.nx_i = activeSet->size[0] - 1;
8586 cartesian_trajectory_planner_B.idx = 0;
8587 for (cartesian_trajectory_planner_B.b_i = 0;
8588 cartesian_trajectory_planner_B.b_i <=
8589 cartesian_trajectory_planner_B.nx_i;
8590 cartesian_trajectory_planner_B.b_i++) {
8591 if (activeSet->data[cartesian_trajectory_planner_B.b_i]) {
8592 cartesian_trajectory_planner_B.idx++;
8593 }
8594 }
8595
8596 cartesian_trajectory_planner_B.b_i = gb->size[0];
8597 gb->size[0] = cartesian_trajectory_planner_B.idx;
8598 carte_emxEnsureCapacity_int32_T(gb,
8599 cartesian_trajectory_planner_B.b_i);
8600 cartesian_trajectory_planner_B.idx = 0;
8601 for (cartesian_trajectory_planner_B.b_i = 0;
8602 cartesian_trajectory_planner_B.b_i <=
8603 cartesian_trajectory_planner_B.nx_i;
8604 cartesian_trajectory_planner_B.b_i++) {
8605 if (activeSet->data[cartesian_trajectory_planner_B.b_i]) {
8606 gb->data[cartesian_trajectory_planner_B.idx] =
8607 cartesian_trajectory_planner_B.b_i + 1;
8608 cartesian_trajectory_planner_B.idx++;
8609 }
8610 }
8611
8612 cartesian_trajectory_planner_B.i_e = obj->ConstraintMatrix->size[0];
8613 cartesian_trajectory_planner_B.b_i = A->size[0] * A->size[1];
8614 A->size[0] = cartesian_trajectory_planner_B.i_e;
8615 A->size[1] = gb->size[0];
8616 cartes_emxEnsureCapacity_real_T(A,
8617 cartesian_trajectory_planner_B.b_i);
8618 cartesian_trajectory_planner_B.n_f = gb->size[0];
8619 for (cartesian_trajectory_planner_B.b_i = 0;
8620 cartesian_trajectory_planner_B.b_i <
8621 cartesian_trajectory_planner_B.n_f;
8622 cartesian_trajectory_planner_B.b_i++) {
8623 for (cartesian_trajectory_planner_B.idx = 0;
8624 cartesian_trajectory_planner_B.idx <
8625 cartesian_trajectory_planner_B.i_e;
8626 cartesian_trajectory_planner_B.idx++) {
8627 A->data[cartesian_trajectory_planner_B.idx + A->size[0] *
8628 cartesian_trajectory_planner_B.b_i] = obj->
8629 ConstraintMatrix->data[(gb->
8630 data[cartesian_trajectory_planner_B.b_i] - 1) *
8631 obj->ConstraintMatrix->size[0] +
8632 cartesian_trajectory_planner_B.idx];
8633 }
8634 }
8635
8636 cartesian_trajectory_planner_B.b_i = AIn->size[0] * AIn->size[1];
8637 AIn->size[0] = grad->size[0];
8638 AIn->size[1] = grad->size[0];
8639 cartes_emxEnsureCapacity_real_T(AIn,
8640 cartesian_trajectory_planner_B.b_i);
8641 cartesian_trajectory_planner_B.i_e = grad->size[0];
8642 for (cartesian_trajectory_planner_B.b_i = 0;
8643 cartesian_trajectory_planner_B.b_i <
8644 cartesian_trajectory_planner_B.i_e;
8645 cartesian_trajectory_planner_B.b_i++) {
8646 cartesian_trajectory_planner_B.n_f = grad->size[0];
8647 for (cartesian_trajectory_planner_B.idx = 0;
8648 cartesian_trajectory_planner_B.idx <
8649 cartesian_trajectory_planner_B.n_f;
8650 cartesian_trajectory_planner_B.idx++) {
8651 AIn->data[cartesian_trajectory_planner_B.idx + AIn->size[0] *
8652 cartesian_trajectory_planner_B.b_i] = grad->
8653 data[cartesian_trajectory_planner_B.idx] * grad->
8654 data[cartesian_trajectory_planner_B.b_i];
8655 }
8656 }
8657
8658 cartesian_trajectory_planner_B.m_p = AIn->size[0] - 1;
8659 cartesian_trajectory_planner_B.inner = AIn->size[1] - 1;
8660 cartesian_trajectory_planner_B.b_i = y_0->size[0] * y_0->size[1];
8661 y_0->size[0] = AIn->size[0];
8662 y_0->size[1] = 6;
8663 cartes_emxEnsureCapacity_real_T(y_0,
8664 cartesian_trajectory_planner_B.b_i);
8665 for (cartesian_trajectory_planner_B.idx = 0;
8666 cartesian_trajectory_planner_B.idx < 6;
8667 cartesian_trajectory_planner_B.idx++) {
8668 cartesian_trajectory_planner_B.coffset =
8669 (cartesian_trajectory_planner_B.m_p + 1) *
8670 cartesian_trajectory_planner_B.idx - 1;
8671 cartesian_trajectory_planner_B.boffset =
8672 cartesian_trajectory_planner_B.idx * 6 - 1;
8673 for (cartesian_trajectory_planner_B.b_i = 0;
8674 cartesian_trajectory_planner_B.b_i <=
8675 cartesian_trajectory_planner_B.m_p;
8676 cartesian_trajectory_planner_B.b_i++) {
8677 y_0->data[(cartesian_trajectory_planner_B.coffset +
8678 cartesian_trajectory_planner_B.b_i) + 1] = 0.0;
8679 }
8680
8681 for (cartesian_trajectory_planner_B.nx_i = 0;
8682 cartesian_trajectory_planner_B.nx_i <=
8683 cartesian_trajectory_planner_B.inner;
8684 cartesian_trajectory_planner_B.nx_i++) {
8685 cartesian_trajectory_planner_B.aoffset =
8686 cartesian_trajectory_planner_B.nx_i * AIn->size[0] - 1;
8687 cartesian_trajectory_planner_B.s_j =
8688 cartesian_trajectory_planner_B.H
8689 [(cartesian_trajectory_planner_B.boffset +
8690 cartesian_trajectory_planner_B.nx_i) + 1];
8691 for (cartesian_trajectory_planner_B.j = 0;
8692 cartesian_trajectory_planner_B.j <=
8693 cartesian_trajectory_planner_B.m_p;
8694 cartesian_trajectory_planner_B.j++) {
8695 cartesian_trajectory_planner_B.i_e =
8696 cartesian_trajectory_planner_B.j + 1;
8697 cartesian_trajectory_planner_B.b_i =
8698 cartesian_trajectory_planner_B.coffset +
8699 cartesian_trajectory_planner_B.i_e;
8700 y_0->data[cartesian_trajectory_planner_B.b_i] += AIn->
8701 data[cartesian_trajectory_planner_B.aoffset +
8702 cartesian_trajectory_planner_B.i_e] *
8703 cartesian_trajectory_planner_B.s_j;
8704 }
8705 }
8706 }
8707
8708 cartesian_trajectory_planner_B.b_i = grad_1->size[0] *
8709 grad_1->size[1];
8710 grad_1->size[0] = 1;
8711 grad_1->size[1] = grad->size[0];
8712 cartes_emxEnsureCapacity_real_T(grad_1,
8713 cartesian_trajectory_planner_B.b_i);
8714 cartesian_trajectory_planner_B.i_e = grad->size[0];
8715 for (cartesian_trajectory_planner_B.b_i = 0;
8716 cartesian_trajectory_planner_B.b_i <
8717 cartesian_trajectory_planner_B.i_e;
8718 cartesian_trajectory_planner_B.b_i++) {
8719 grad_1->data[cartesian_trajectory_planner_B.b_i] = grad->
8720 data[cartesian_trajectory_planner_B.b_i];
8721 }
8722
8723 cartesian_trajectory_planner_B.beta = 0.0;
8724 for (cartesian_trajectory_planner_B.b_i = 0;
8725 cartesian_trajectory_planner_B.b_i < 6;
8726 cartesian_trajectory_planner_B.b_i++) {
8727 cartesian_trajectory_planner_B.sNew_g[cartesian_trajectory_planner_B.b_i]
8728 = 0.0;
8729 for (cartesian_trajectory_planner_B.idx = 0;
8730 cartesian_trajectory_planner_B.idx < 6;
8731 cartesian_trajectory_planner_B.idx++) {
8732 cartesian_trajectory_planner_B.sigma =
8733 cartesian_trajectory_planner_B.H[6 *
8734 cartesian_trajectory_planner_B.b_i +
8735 cartesian_trajectory_planner_B.idx] * grad_1->
8736 data[cartesian_trajectory_planner_B.idx] +
8737 cartesian_trajectory_planner_B.sNew_g[cartesian_trajectory_planner_B.b_i];
8738 cartesian_trajectory_planner_B.sNew_g[cartesian_trajectory_planner_B.b_i]
8739 = cartesian_trajectory_planner_B.sigma;
8740 }
8741
8742 cartesian_trajectory_planner_B.beta +=
8743 cartesian_trajectory_planner_B.sNew_g[cartesian_trajectory_planner_B.b_i]
8744 * grad->data[cartesian_trajectory_planner_B.b_i];
8745 }
8746
8747 cartesian_trajectory_planner_B.s_j = 1.0 /
8748 cartesian_trajectory_planner_B.beta;
8749 for (cartesian_trajectory_planner_B.b_i = 0;
8750 cartesian_trajectory_planner_B.b_i < 6;
8751 cartesian_trajectory_planner_B.b_i++) {
8752 for (cartesian_trajectory_planner_B.idx = 0;
8753 cartesian_trajectory_planner_B.idx < 6;
8754 cartesian_trajectory_planner_B.idx++) {
8755 cartesian_trajectory_planner_B.idxl =
8756 cartesian_trajectory_planner_B.b_i + 6 *
8757 cartesian_trajectory_planner_B.idx;
8758 cartesian_trajectory_planner_B.H_m[cartesian_trajectory_planner_B.idxl]
8759 = 0.0;
8760 for (cartesian_trajectory_planner_B.i_e = 0;
8761 cartesian_trajectory_planner_B.i_e < 6;
8762 cartesian_trajectory_planner_B.i_e++) {
8763 cartesian_trajectory_planner_B.H_m[cartesian_trajectory_planner_B.idxl]
8764 += cartesian_trajectory_planner_B.H[6 *
8765 cartesian_trajectory_planner_B.i_e +
8766 cartesian_trajectory_planner_B.b_i] * y_0->data[6 *
8767 cartesian_trajectory_planner_B.idx +
8768 cartesian_trajectory_planner_B.i_e];
8769 }
8770 }
8771 }
8772
8773 for (cartesian_trajectory_planner_B.b_i = 0;
8774 cartesian_trajectory_planner_B.b_i < 36;
8775 cartesian_trajectory_planner_B.b_i++) {
8776 cartesian_trajectory_planner_B.H[cartesian_trajectory_planner_B.b_i]
8777 -= cartesian_trajectory_planner_B.s_j *
8778 cartesian_trajectory_planner_B.H_m[cartesian_trajectory_planner_B.b_i];
8779 }
8780
8781 guard1 = true;
8782 } else {
8783 cartesian_trajectory_planner_B.b_i = grad->size[0];
8784 grad->size[0] = alpha->size[0];
8785 cartes_emxEnsureCapacity_real_T(grad,
8786 cartesian_trajectory_planner_B.b_i);
8787 cartesian_trajectory_planner_B.i_e = alpha->size[0];
8788 for (cartesian_trajectory_planner_B.b_i = 0;
8789 cartesian_trajectory_planner_B.b_i <
8790 cartesian_trajectory_planner_B.i_e;
8791 cartesian_trajectory_planner_B.b_i++) {
8792 grad->data[cartesian_trajectory_planner_B.b_i] = alpha->
8793 data[cartesian_trajectory_planner_B.b_i] - grad->
8794 data[cartesian_trajectory_planner_B.b_i];
8795 }
8796
8797 cartesian_trajectory_planner_B.b_gamma = 0.0;
8798 for (cartesian_trajectory_planner_B.b_i = 0;
8799 cartesian_trajectory_planner_B.b_i < 6;
8800 cartesian_trajectory_planner_B.b_i++) {
8801 cartesian_trajectory_planner_B.b_gamma +=
8802 cartesian_trajectory_planner_B.Hg[cartesian_trajectory_planner_B.b_i]
8803 * grad->data[cartesian_trajectory_planner_B.b_i];
8804 }
8805
8806 cartesian_trajectory_planner_B.b_i = tmp->size[0] * tmp->size[1];
8807 tmp->size[0] = 1;
8808 tmp->size[1] = grad->size[0];
8809 cartes_emxEnsureCapacity_real_T(tmp,
8810 cartesian_trajectory_planner_B.b_i);
8811 cartesian_trajectory_planner_B.i_e = grad->size[0];
8812 for (cartesian_trajectory_planner_B.b_i = 0;
8813 cartesian_trajectory_planner_B.b_i <
8814 cartesian_trajectory_planner_B.i_e;
8815 cartesian_trajectory_planner_B.b_i++) {
8816 tmp->data[cartesian_trajectory_planner_B.b_i] = 0.2 * grad->
8817 data[cartesian_trajectory_planner_B.b_i];
8818 }
8819
8820 cartesian_trajectory_planner_B.s_j = 0.0;
8821 for (cartesian_trajectory_planner_B.b_i = 0;
8822 cartesian_trajectory_planner_B.b_i < 6;
8823 cartesian_trajectory_planner_B.b_i++) {
8824 cartesian_trajectory_planner_B.sNew_g[cartesian_trajectory_planner_B.b_i]
8825 = 0.0;
8826 for (cartesian_trajectory_planner_B.idx = 0;
8827 cartesian_trajectory_planner_B.idx < 6;
8828 cartesian_trajectory_planner_B.idx++) {
8829 cartesian_trajectory_planner_B.beta =
8830 cartesian_trajectory_planner_B.H[6 *
8831 cartesian_trajectory_planner_B.b_i +
8832 cartesian_trajectory_planner_B.idx] * tmp->
8833 data[cartesian_trajectory_planner_B.idx] +
8834 cartesian_trajectory_planner_B.sNew_g[cartesian_trajectory_planner_B.b_i];
8835 cartesian_trajectory_planner_B.sNew_g[cartesian_trajectory_planner_B.b_i]
8836 = cartesian_trajectory_planner_B.beta;
8837 }
8838
8839 cartesian_trajectory_planner_B.s_j +=
8840 cartesian_trajectory_planner_B.sNew_g[cartesian_trajectory_planner_B.b_i]
8841 * grad->data[cartesian_trajectory_planner_B.b_i];
8842 }
8843
8844 if (cartesian_trajectory_planner_B.b_gamma <
8845 cartesian_trajectory_planner_B.s_j) {
8846 cartesian_trajectory_planner_B.b_i = tmp_0->size[0] *
8847 tmp_0->size[1];
8848 tmp_0->size[0] = 1;
8849 tmp_0->size[1] = grad->size[0];
8850 cartes_emxEnsureCapacity_real_T(tmp_0,
8851 cartesian_trajectory_planner_B.b_i);
8852 cartesian_trajectory_planner_B.i_e = grad->size[0];
8853 for (cartesian_trajectory_planner_B.b_i = 0;
8854 cartesian_trajectory_planner_B.b_i <
8855 cartesian_trajectory_planner_B.i_e;
8856 cartesian_trajectory_planner_B.b_i++) {
8857 tmp_0->data[cartesian_trajectory_planner_B.b_i] = 0.8 *
8858 grad->data[cartesian_trajectory_planner_B.b_i];
8859 }
8860
8861 cartesian_trajectory_planner_B.s_j = 0.0;
8862 for (cartesian_trajectory_planner_B.b_i = 0;
8863 cartesian_trajectory_planner_B.b_i < 6;
8864 cartesian_trajectory_planner_B.b_i++) {
8865 cartesian_trajectory_planner_B.sNew_g[cartesian_trajectory_planner_B.b_i]
8866 = 0.0;
8867 for (cartesian_trajectory_planner_B.idx = 0;
8868 cartesian_trajectory_planner_B.idx < 6;
8869 cartesian_trajectory_planner_B.idx++) {
8870 cartesian_trajectory_planner_B.beta =
8871 cartesian_trajectory_planner_B.H[6 *
8872 cartesian_trajectory_planner_B.b_i +
8873 cartesian_trajectory_planner_B.idx] * tmp_0->
8874 data[cartesian_trajectory_planner_B.idx] +
8875 cartesian_trajectory_planner_B.sNew_g[cartesian_trajectory_planner_B.b_i];
8876 cartesian_trajectory_planner_B.sNew_g[cartesian_trajectory_planner_B.b_i]
8877 = cartesian_trajectory_planner_B.beta;
8878 }
8879
8880 cartesian_trajectory_planner_B.s_j +=
8881 cartesian_trajectory_planner_B.sNew_g[cartesian_trajectory_planner_B.b_i]
8882 * grad->data[cartesian_trajectory_planner_B.b_i];
8883 }
8884
8885 cartesian_trajectory_planner_B.b_i = grad_0->size[0] *
8886 grad_0->size[1];
8887 grad_0->size[0] = 1;
8888 grad_0->size[1] = grad->size[0];
8889 cartes_emxEnsureCapacity_real_T(grad_0,
8890 cartesian_trajectory_planner_B.b_i);
8891 cartesian_trajectory_planner_B.i_e = grad->size[0];
8892 for (cartesian_trajectory_planner_B.b_i = 0;
8893 cartesian_trajectory_planner_B.b_i <
8894 cartesian_trajectory_planner_B.i_e;
8895 cartesian_trajectory_planner_B.b_i++) {
8896 grad_0->data[cartesian_trajectory_planner_B.b_i] = grad->
8897 data[cartesian_trajectory_planner_B.b_i];
8898 }
8899
8900 cartesian_trajectory_planner_B.beta = 0.0;
8901 cartesian_trajectory_planner_B.b_gamma = 0.0;
8902 for (cartesian_trajectory_planner_B.b_i = 0;
8903 cartesian_trajectory_planner_B.b_i < 6;
8904 cartesian_trajectory_planner_B.b_i++) {
8905 cartesian_trajectory_planner_B.sNew_g[cartesian_trajectory_planner_B.b_i]
8906 = 0.0;
8907 for (cartesian_trajectory_planner_B.idx = 0;
8908 cartesian_trajectory_planner_B.idx < 6;
8909 cartesian_trajectory_planner_B.idx++) {
8910 cartesian_trajectory_planner_B.sigma =
8911 cartesian_trajectory_planner_B.H[6 *
8912 cartesian_trajectory_planner_B.b_i +
8913 cartesian_trajectory_planner_B.idx] * grad_0->
8914 data[cartesian_trajectory_planner_B.idx] +
8915 cartesian_trajectory_planner_B.sNew_g[cartesian_trajectory_planner_B.b_i];
8916 cartesian_trajectory_planner_B.sNew_g[cartesian_trajectory_planner_B.b_i]
8917 = cartesian_trajectory_planner_B.sigma;
8918 }
8919
8920 cartesian_trajectory_planner_B.beta +=
8921 cartesian_trajectory_planner_B.sNew_g[cartesian_trajectory_planner_B.b_i]
8922 * grad->data[cartesian_trajectory_planner_B.b_i];
8923 cartesian_trajectory_planner_B.b_gamma +=
8924 cartesian_trajectory_planner_B.Hg[cartesian_trajectory_planner_B.b_i]
8925 * grad->data[cartesian_trajectory_planner_B.b_i];
8926 }
8927
8928 cartesian_trajectory_planner_B.b_gamma =
8929 cartesian_trajectory_planner_B.s_j /
8930 (cartesian_trajectory_planner_B.beta -
8931 cartesian_trajectory_planner_B.b_gamma);
8932 } else {
8933 cartesian_trajectory_planner_B.b_gamma = 1.0;
8934 }
8935
8936 cartesian_trajectory_planner_B.beta = 0.0;
8937 for (cartesian_trajectory_planner_B.b_i = 0;
8938 cartesian_trajectory_planner_B.b_i < 6;
8939 cartesian_trajectory_planner_B.b_i++) {
8940 cartesian_trajectory_planner_B.s_j = 0.0;
8941 for (cartesian_trajectory_planner_B.idx = 0;
8942 cartesian_trajectory_planner_B.idx < 6;
8943 cartesian_trajectory_planner_B.idx++) {
8944 cartesian_trajectory_planner_B.s_j +=
8945 cartesian_trajectory_planner_B.H[6 *
8946 cartesian_trajectory_planner_B.idx +
8947 cartesian_trajectory_planner_B.b_i] * (1.0 -
8948 cartesian_trajectory_planner_B.b_gamma) * grad->
8949 data[cartesian_trajectory_planner_B.idx];
8950 }
8951
8952 cartesian_trajectory_planner_B.s_j +=
8953 cartesian_trajectory_planner_B.b_gamma *
8954 cartesian_trajectory_planner_B.Hg[cartesian_trajectory_planner_B.b_i];
8955 cartesian_trajectory_planner_B.beta +=
8956 cartesian_trajectory_planner_B.s_j * grad->
8957 data[cartesian_trajectory_planner_B.b_i];
8958 cartesian_trajectory_planner_B.sNew_g[cartesian_trajectory_planner_B.b_i]
8959 = cartesian_trajectory_planner_B.s_j;
8960 }
8961
8962 cartesian_trajectory_planner_B.b_i = sNew->size[0] * sNew->size[1];
8963 sNew->size[0] = 6;
8964 sNew->size[1] = grad->size[0];
8965 cartes_emxEnsureCapacity_real_T(sNew,
8966 cartesian_trajectory_planner_B.b_i);
8967 cartesian_trajectory_planner_B.i_e = grad->size[0];
8968 for (cartesian_trajectory_planner_B.b_i = 0;
8969 cartesian_trajectory_planner_B.b_i <
8970 cartesian_trajectory_planner_B.i_e;
8971 cartesian_trajectory_planner_B.b_i++) {
8972 for (cartesian_trajectory_planner_B.idx = 0;
8973 cartesian_trajectory_planner_B.idx < 6;
8974 cartesian_trajectory_planner_B.idx++) {
8975 cartesian_trajectory_planner_B.s_j =
8976 cartesian_trajectory_planner_B.sNew_g[cartesian_trajectory_planner_B.idx]
8977 * grad->data[cartesian_trajectory_planner_B.b_i];
8978 sNew->data[cartesian_trajectory_planner_B.idx + 6 *
8979 cartesian_trajectory_planner_B.b_i] =
8980 cartesian_trajectory_planner_B.s_j /
8981 cartesian_trajectory_planner_B.beta;
8982 }
8983 }
8984
8985 for (cartesian_trajectory_planner_B.b_i = 0;
8986 cartesian_trajectory_planner_B.b_i < 36;
8987 cartesian_trajectory_planner_B.b_i++) {
8988 cartesian_trajectory_planner_B.V[cartesian_trajectory_planner_B.b_i]
8989 =
8990 cartesian_trajectory_planner_B.unusedU0[cartesian_trajectory_planner_B.b_i]
8991 - sNew->data[cartesian_trajectory_planner_B.b_i];
8992 }
8993
8994 for (cartesian_trajectory_planner_B.b_i = 0;
8995 cartesian_trajectory_planner_B.b_i < 6;
8996 cartesian_trajectory_planner_B.b_i++) {
8997 for (cartesian_trajectory_planner_B.idx = 0;
8998 cartesian_trajectory_planner_B.idx < 6;
8999 cartesian_trajectory_planner_B.idx++) {
9000 cartesian_trajectory_planner_B.nx_i =
9001 cartesian_trajectory_planner_B.b_i + 6 *
9002 cartesian_trajectory_planner_B.idx;
9003 cartesian_trajectory_planner_B.H_m[cartesian_trajectory_planner_B.nx_i]
9004 = 0.0;
9005 for (cartesian_trajectory_planner_B.i_e = 0;
9006 cartesian_trajectory_planner_B.i_e < 6;
9007 cartesian_trajectory_planner_B.i_e++) {
9008 cartesian_trajectory_planner_B.H_m[cartesian_trajectory_planner_B.nx_i]
9009 += cartesian_trajectory_planner_B.V[6 *
9010 cartesian_trajectory_planner_B.i_e +
9011 cartesian_trajectory_planner_B.b_i] *
9012 cartesian_trajectory_planner_B.H[6 *
9013 cartesian_trajectory_planner_B.idx +
9014 cartesian_trajectory_planner_B.i_e];
9015 }
9016 }
9017 }
9018
9019 for (cartesian_trajectory_planner_B.b_i = 0;
9020 cartesian_trajectory_planner_B.b_i < 6;
9021 cartesian_trajectory_planner_B.b_i++) {
9022 for (cartesian_trajectory_planner_B.idx = 0;
9023 cartesian_trajectory_planner_B.idx < 6;
9024 cartesian_trajectory_planner_B.idx++) {
9025 cartesian_trajectory_planner_B.nx_i =
9026 cartesian_trajectory_planner_B.b_i + 6 *
9027 cartesian_trajectory_planner_B.idx;
9028 cartesian_trajectory_planner_B.P[cartesian_trajectory_planner_B.nx_i]
9029 = 0.0;
9030 for (cartesian_trajectory_planner_B.i_e = 0;
9031 cartesian_trajectory_planner_B.i_e < 6;
9032 cartesian_trajectory_planner_B.i_e++) {
9033 cartesian_trajectory_planner_B.P[cartesian_trajectory_planner_B.nx_i]
9034 += cartesian_trajectory_planner_B.H_m[6 *
9035 cartesian_trajectory_planner_B.i_e +
9036 cartesian_trajectory_planner_B.b_i] *
9037 cartesian_trajectory_planner_B.V[6 *
9038 cartesian_trajectory_planner_B.i_e +
9039 cartesian_trajectory_planner_B.idx];
9040 }
9041 }
9042 }
9043
9044 for (cartesian_trajectory_planner_B.b_i = 0;
9045 cartesian_trajectory_planner_B.b_i < 6;
9046 cartesian_trajectory_planner_B.b_i++) {
9047 for (cartesian_trajectory_planner_B.idx = 0;
9048 cartesian_trajectory_planner_B.idx < 6;
9049 cartesian_trajectory_planner_B.idx++) {
9050 cartesian_trajectory_planner_B.sNew[cartesian_trajectory_planner_B.idx
9051 + 6 * cartesian_trajectory_planner_B.b_i] =
9052 cartesian_trajectory_planner_B.sNew_g[cartesian_trajectory_planner_B.idx]
9053 * cartesian_trajectory_planner_B.sNew_g[cartesian_trajectory_planner_B.b_i]
9054 / cartesian_trajectory_planner_B.beta;
9055 }
9056 }
9057
9058 for (cartesian_trajectory_planner_B.b_i = 0;
9059 cartesian_trajectory_planner_B.b_i < 36;
9060 cartesian_trajectory_planner_B.b_i++) {
9061 cartesian_trajectory_planner_B.s_j =
9062 cartesian_trajectory_planner_B.P[cartesian_trajectory_planner_B.b_i]
9063 + cartesian_trajectory_planner_B.sNew[cartesian_trajectory_planner_B.b_i];
9064 cartesian_trajectory_planner_B.H_m[cartesian_trajectory_planner_B.b_i]
9065 = 1.4901161193847656E-8 * static_cast<real_T>
9066 (tmp_1[cartesian_trajectory_planner_B.b_i]) +
9067 cartesian_trajectory_planner_B.s_j;
9068 cartesian_trajectory_planner_B.H[cartesian_trajectory_planner_B.b_i]
9069 = cartesian_trajectory_planner_B.s_j;
9070 }
9071
9072 if (!cartesian_tr_isPositiveDefinite
9073 (cartesian_trajectory_planner_B.H_m)) {
9074 *exitFlag = HessianNotPositiveSemidefinite;
9075 args = obj->ExtraArgs;
9076 for (cartesian_trajectory_planner_B.b_i = 0;
9077 cartesian_trajectory_planner_B.b_i < 36;
9078 cartesian_trajectory_planner_B.b_i++) {
9079 cartesian_trajectory_planner_B.unusedU0[cartesian_trajectory_planner_B.b_i]
9080 = args->WeightMatrix[cartesian_trajectory_planner_B.b_i];
9081 }
9082
9083 cartesian_trajectory_planner_B.b_i = grad->size[0];
9084 grad->size[0] = args->ErrTemp->size[0];
9085 cartes_emxEnsureCapacity_real_T(grad,
9086 cartesian_trajectory_planner_B.b_i);
9087 cartesian_trajectory_planner_B.i_e = args->ErrTemp->size[0];
9088 for (cartesian_trajectory_planner_B.b_i = 0;
9089 cartesian_trajectory_planner_B.b_i <
9090 cartesian_trajectory_planner_B.i_e;
9091 cartesian_trajectory_planner_B.b_i++) {
9092 grad->data[cartesian_trajectory_planner_B.b_i] = args->
9093 ErrTemp->data[cartesian_trajectory_planner_B.b_i];
9094 }
9095
9096 for (cartesian_trajectory_planner_B.b_i = 0;
9097 cartesian_trajectory_planner_B.b_i < 6;
9098 cartesian_trajectory_planner_B.b_i++) {
9099 cartesian_trajectory_planner_B.x[cartesian_trajectory_planner_B.b_i]
9100 = 0.0;
9101 for (cartesian_trajectory_planner_B.idx = 0;
9102 cartesian_trajectory_planner_B.idx < 6;
9103 cartesian_trajectory_planner_B.idx++) {
9104 cartesian_trajectory_planner_B.A_i =
9105 cartesian_trajectory_planner_B.unusedU0[6 *
9106 cartesian_trajectory_planner_B.idx +
9107 cartesian_trajectory_planner_B.b_i] * grad->
9108 data[cartesian_trajectory_planner_B.idx] +
9109 cartesian_trajectory_planner_B.x[cartesian_trajectory_planner_B.b_i];
9110 cartesian_trajectory_planner_B.x[cartesian_trajectory_planner_B.b_i]
9111 = cartesian_trajectory_planner_B.A_i;
9112 }
9113 }
9114
9115 *err = cartesian_trajectory_pla_norm_a
9116 (cartesian_trajectory_planner_B.x);
9117 *iter = static_cast<real_T>
9118 (cartesian_trajectory_planner_B.g_idx_0) + 1.0;
9119 exitg2 = 1;
9120 } else {
9121 guard1 = true;
9122 }
9123 }
9124 }
9125
9126 if (guard1) {
9127 if (DampedBFGSwGradientProjectio_as(obj, xSol)) {
9128 for (cartesian_trajectory_planner_B.i_e = 0;
9129 cartesian_trajectory_planner_B.i_e < 6;
9130 cartesian_trajectory_planner_B.i_e++) {
9131 xSol[cartesian_trajectory_planner_B.i_e] =
9132 cartesian_trajectory_planner_B.x[cartesian_trajectory_planner_B.i_e];
9133 }
9134
9135 *exitFlag = SearchDirectionInvalid;
9136 args = obj->ExtraArgs;
9137 for (cartesian_trajectory_planner_B.b_i = 0;
9138 cartesian_trajectory_planner_B.b_i < 36;
9139 cartesian_trajectory_planner_B.b_i++) {
9140 cartesian_trajectory_planner_B.unusedU0[cartesian_trajectory_planner_B.b_i]
9141 = args->WeightMatrix[cartesian_trajectory_planner_B.b_i];
9142 }
9143
9144 cartesian_trajectory_planner_B.b_i = grad->size[0];
9145 grad->size[0] = args->ErrTemp->size[0];
9146 cartes_emxEnsureCapacity_real_T(grad,
9147 cartesian_trajectory_planner_B.b_i);
9148 cartesian_trajectory_planner_B.i_e = args->ErrTemp->size[0];
9149 for (cartesian_trajectory_planner_B.b_i = 0;
9150 cartesian_trajectory_planner_B.b_i <
9151 cartesian_trajectory_planner_B.i_e;
9152 cartesian_trajectory_planner_B.b_i++) {
9153 grad->data[cartesian_trajectory_planner_B.b_i] = args->
9154 ErrTemp->data[cartesian_trajectory_planner_B.b_i];
9155 }
9156
9157 for (cartesian_trajectory_planner_B.b_i = 0;
9158 cartesian_trajectory_planner_B.b_i < 6;
9159 cartesian_trajectory_planner_B.b_i++) {
9160 cartesian_trajectory_planner_B.x[cartesian_trajectory_planner_B.b_i]
9161 = 0.0;
9162 for (cartesian_trajectory_planner_B.idx = 0;
9163 cartesian_trajectory_planner_B.idx < 6;
9164 cartesian_trajectory_planner_B.idx++) {
9165 cartesian_trajectory_planner_B.A_i =
9166 cartesian_trajectory_planner_B.unusedU0[6 *
9167 cartesian_trajectory_planner_B.idx +
9168 cartesian_trajectory_planner_B.b_i] * grad->
9169 data[cartesian_trajectory_planner_B.idx] +
9170 cartesian_trajectory_planner_B.x[cartesian_trajectory_planner_B.b_i];
9171 cartesian_trajectory_planner_B.x[cartesian_trajectory_planner_B.b_i]
9172 = cartesian_trajectory_planner_B.A_i;
9173 }
9174 }
9175
9176 *err = cartesian_trajectory_pla_norm_a
9177 (cartesian_trajectory_planner_B.x);
9178 *iter = static_cast<real_T>(cartesian_trajectory_planner_B.g_idx_0)
9179 + 1.0;
9180 exitg2 = 1;
9181 } else {
9182 for (cartesian_trajectory_planner_B.i_e = 0;
9183 cartesian_trajectory_planner_B.i_e < 6;
9184 cartesian_trajectory_planner_B.i_e++) {
9185 cartesian_trajectory_planner_B.x[cartesian_trajectory_planner_B.i_e]
9186 = xSol[cartesian_trajectory_planner_B.i_e];
9187 }
9188
9189 cartesian_trajectory_planner_B.b_i = grad->size[0];
9190 grad->size[0] = alpha->size[0];
9191 cartes_emxEnsureCapacity_real_T(grad,
9192 cartesian_trajectory_planner_B.b_i);
9193 cartesian_trajectory_planner_B.i_e = alpha->size[0];
9194 for (cartesian_trajectory_planner_B.b_i = 0;
9195 cartesian_trajectory_planner_B.b_i <
9196 cartesian_trajectory_planner_B.i_e;
9197 cartesian_trajectory_planner_B.b_i++) {
9198 grad->data[cartesian_trajectory_planner_B.b_i] = alpha->
9199 data[cartesian_trajectory_planner_B.b_i];
9200 }
9201
9202 cartesian_trajectory_planner_B.cost =
9203 cartesian_trajectory_planner_B.costNew;
9204 cartesian_trajectory_planner_B.g_idx_0++;
9205 }
9206 }
9207 }
9208 }
9209 } else {
9210 *exitFlag = IterationLimitExceeded;
9211 args = obj->ExtraArgs;
9212 for (cartesian_trajectory_planner_B.b_i = 0;
9213 cartesian_trajectory_planner_B.b_i < 36;
9214 cartesian_trajectory_planner_B.b_i++) {
9215 cartesian_trajectory_planner_B.unusedU0[cartesian_trajectory_planner_B.b_i]
9216 = args->WeightMatrix[cartesian_trajectory_planner_B.b_i];
9217 }
9218
9219 cartesian_trajectory_planner_B.b_i = grad->size[0];
9220 grad->size[0] = args->ErrTemp->size[0];
9221 cartes_emxEnsureCapacity_real_T(grad, cartesian_trajectory_planner_B.b_i);
9222 cartesian_trajectory_planner_B.i_e = args->ErrTemp->size[0];
9223 for (cartesian_trajectory_planner_B.b_i = 0;
9224 cartesian_trajectory_planner_B.b_i <
9225 cartesian_trajectory_planner_B.i_e;
9226 cartesian_trajectory_planner_B.b_i++) {
9227 grad->data[cartesian_trajectory_planner_B.b_i] = args->ErrTemp->
9228 data[cartesian_trajectory_planner_B.b_i];
9229 }
9230
9231 for (cartesian_trajectory_planner_B.b_i = 0;
9232 cartesian_trajectory_planner_B.b_i < 6;
9233 cartesian_trajectory_planner_B.b_i++) {
9234 cartesian_trajectory_planner_B.x[cartesian_trajectory_planner_B.b_i] =
9235 0.0;
9236 for (cartesian_trajectory_planner_B.idx = 0;
9237 cartesian_trajectory_planner_B.idx < 6;
9238 cartesian_trajectory_planner_B.idx++) {
9239 cartesian_trajectory_planner_B.A_i =
9240 cartesian_trajectory_planner_B.unusedU0[6 *
9241 cartesian_trajectory_planner_B.idx +
9242 cartesian_trajectory_planner_B.b_i] * grad->
9243 data[cartesian_trajectory_planner_B.idx] +
9244 cartesian_trajectory_planner_B.x[cartesian_trajectory_planner_B.b_i];
9245 cartesian_trajectory_planner_B.x[cartesian_trajectory_planner_B.b_i] =
9246 cartesian_trajectory_planner_B.A_i;
9247 }
9248 }
9249
9250 *err = cartesian_trajectory_pla_norm_a(cartesian_trajectory_planner_B.x);
9251 *iter = obj->MaxNumIterationInternal;
9252 exitg2 = 1;
9253 }
9254 } while (exitg2 == 0);
9255
9256 cartesian_traje_emxFree_int32_T(&ii_3);
9257 cartesian_trajec_emxFree_real_T(&alpha_0);
9258 cartesian_trajec_emxFree_real_T(&A_3);
9259 cartesian_trajec_emxFree_real_T(&grad_1);
9260 cartesian_traje_emxFree_int32_T(&ii_2);
9261 cartesian_traje_emxFree_int32_T(&ii_1);
9262 cartesian_trajec_emxFree_real_T(&sNew);
9263 cartesian_trajec_emxFree_real_T(&grad_0);
9264 cartesian_trajec_emxFree_real_T(&tmp_0);
9265 cartesian_trajec_emxFree_real_T(&tmp);
9266 cartesian_trajec_emxFree_real_T(&sigma);
9267 cartesian_trajec_emxFree_real_T(&A_2);
9268 cartesian_tra_emxFree_boolean_T(&x);
9269 cartesian_trajec_emxFree_real_T(&y_0);
9270 cartesian_traje_emxFree_int32_T(&ii_0);
9271 cartesian_trajec_emxFree_real_T(&y);
9272 cartesian_traje_emxFree_int32_T(&ii);
9273 cartesian_trajec_emxFree_real_T(&a);
9274 cartesian_traje_emxFree_int32_T(&gb);
9275 cartesian_traje_emxFree_int32_T(&fb);
9276 cartesian_traje_emxFree_int32_T(&eb);
9277 cartesian_traje_emxFree_int32_T(&db);
9278 cartesian_traje_emxFree_int32_T(&cb);
9279 cartesian_trajec_emxFree_real_T(&L);
9280 cartesian_trajec_emxFree_real_T(&AIn);
9281 cartesian_trajec_emxFree_real_T(&alpha);
9282 cartesian_trajec_emxFree_real_T(&A);
9283 cartesian_tra_emxFree_boolean_T(&activeSet);
9284 cartesian_trajec_emxFree_real_T(&grad);
9285 cartesian_trajec_emxFree_real_T(&unusedU1);
9286}
9287
9288static void cartesian_trajectory_p_isfinite(const
9289 emxArray_real_T_cartesian_tra_T *x, emxArray_boolean_T_cartesian__T *b)
9290{
9291 emxArray_boolean_T_cartesian__T *tmp;
9292 int32_T loop_ub;
9293 int32_T i;
9294 i = b->size[0];
9295 b->size[0] = x->size[0];
9296 car_emxEnsureCapacity_boolean_T(b, i);
9297 loop_ub = x->size[0];
9298 for (i = 0; i < loop_ub; i++) {
9299 b->data[i] = rtIsInf(x->data[i]);
9300 }
9301
9302 cartesian_tra_emxInit_boolean_T(&tmp, 1);
9303 i = tmp->size[0];
9304 tmp->size[0] = x->size[0];
9305 car_emxEnsureCapacity_boolean_T(tmp, i);
9306 loop_ub = x->size[0];
9307 for (i = 0; i < loop_ub; i++) {
9308 tmp->data[i] = rtIsNaN(x->data[i]);
9309 }
9310
9311 i = b->size[0];
9312 car_emxEnsureCapacity_boolean_T(b, i);
9313 loop_ub = b->size[0];
9314 for (i = 0; i < loop_ub; i++) {
9315 b->data[i] = ((!b->data[i]) && (!tmp->data[i]));
9316 }
9317
9318 cartesian_tra_emxFree_boolean_T(&tmp);
9319}
9320
9321static void cartesi_genrand_uint32_vector_a(uint32_T mt[625], uint32_T u[2])
9322{
9323 for (cartesian_trajectory_planner_B.b_j_j = 0;
9324 cartesian_trajectory_planner_B.b_j_j < 2;
9325 cartesian_trajectory_planner_B.b_j_j++) {
9326 cartesian_trajectory_planner_B.mti = mt[624] + 1U;
9327 if (cartesian_trajectory_planner_B.mti >= 625U) {
9328 for (cartesian_trajectory_planner_B.b_kk = 0;
9329 cartesian_trajectory_planner_B.b_kk < 227;
9330 cartesian_trajectory_planner_B.b_kk++) {
9331 cartesian_trajectory_planner_B.y_e =
9332 (mt[cartesian_trajectory_planner_B.b_kk + 1] & 2147483647U) |
9333 (mt[cartesian_trajectory_planner_B.b_kk] & 2147483648U);
9334 if ((cartesian_trajectory_planner_B.y_e & 1U) == 0U) {
9335 cartesian_trajectory_planner_B.y_e >>= 1U;
9336 } else {
9337 cartesian_trajectory_planner_B.y_e =
9338 cartesian_trajectory_planner_B.y_e >> 1U ^ 2567483615U;
9339 }
9340
9341 mt[cartesian_trajectory_planner_B.b_kk] =
9342 mt[cartesian_trajectory_planner_B.b_kk + 397] ^
9343 cartesian_trajectory_planner_B.y_e;
9344 }
9345
9346 for (cartesian_trajectory_planner_B.b_kk = 0;
9347 cartesian_trajectory_planner_B.b_kk < 396;
9348 cartesian_trajectory_planner_B.b_kk++) {
9349 cartesian_trajectory_planner_B.y_e =
9350 (mt[cartesian_trajectory_planner_B.b_kk + 227] & 2147483648U) |
9351 (mt[cartesian_trajectory_planner_B.b_kk + 228] & 2147483647U);
9352 if ((cartesian_trajectory_planner_B.y_e & 1U) == 0U) {
9353 cartesian_trajectory_planner_B.y_e >>= 1U;
9354 } else {
9355 cartesian_trajectory_planner_B.y_e =
9356 cartesian_trajectory_planner_B.y_e >> 1U ^ 2567483615U;
9357 }
9358
9359 mt[cartesian_trajectory_planner_B.b_kk + 227] =
9360 mt[cartesian_trajectory_planner_B.b_kk] ^
9361 cartesian_trajectory_planner_B.y_e;
9362 }
9363
9364 cartesian_trajectory_planner_B.y_e = (mt[623] & 2147483648U) | (mt[0] &
9365 2147483647U);
9366 if ((cartesian_trajectory_planner_B.y_e & 1U) == 0U) {
9367 cartesian_trajectory_planner_B.y_e >>= 1U;
9368 } else {
9369 cartesian_trajectory_planner_B.y_e = cartesian_trajectory_planner_B.y_e >>
9370 1U ^ 2567483615U;
9371 }
9372
9373 mt[623] = mt[396] ^ cartesian_trajectory_planner_B.y_e;
9374 cartesian_trajectory_planner_B.mti = 1U;
9375 }
9376
9377 cartesian_trajectory_planner_B.y_e = mt[static_cast<int32_T>
9378 (cartesian_trajectory_planner_B.mti) - 1];
9379 mt[624] = cartesian_trajectory_planner_B.mti;
9380 cartesian_trajectory_planner_B.y_e ^= cartesian_trajectory_planner_B.y_e >>
9381 11U;
9382 cartesian_trajectory_planner_B.y_e ^= cartesian_trajectory_planner_B.y_e <<
9383 7U & 2636928640U;
9384 cartesian_trajectory_planner_B.y_e ^= cartesian_trajectory_planner_B.y_e <<
9385 15U & 4022730752U;
9386 u[cartesian_trajectory_planner_B.b_j_j] = cartesian_trajectory_planner_B.y_e
9387 >> 18U ^ cartesian_trajectory_planner_B.y_e;
9388 }
9389}
9390
9391static boolean_T cartesian_trajec_is_valid_state(const uint32_T mt[625])
9392{
9393 boolean_T isvalid;
9394 boolean_T exitg1;
9395 if ((mt[624] >= 1U) && (mt[624] < 625U)) {
9396 isvalid = true;
9397 } else {
9398 isvalid = false;
9399 }
9400
9401 if (isvalid) {
9402 isvalid = false;
9403 cartesian_trajectory_planner_B.k_h = 0;
9404 exitg1 = false;
9405 while ((!exitg1) && (cartesian_trajectory_planner_B.k_h + 1 < 625)) {
9406 if (mt[cartesian_trajectory_planner_B.k_h] == 0U) {
9407 cartesian_trajectory_planner_B.k_h++;
9408 } else {
9409 isvalid = true;
9410 exitg1 = true;
9411 }
9412 }
9413 }
9414
9415 return isvalid;
9416}
9417
9418static real_T cartesian_trajectory_genrandu_a(uint32_T mt[625])
9419{
9420 real_T r;
9421 int32_T exitg1;
9422
9423 // ========================= COPYRIGHT NOTICE ============================
9424 // This is a uniform (0,1) pseudorandom number generator based on:
9425 //
9426 // A C-program for MT19937, with initialization improved 2002/1/26.
9427 // Coded by Takuji Nishimura and Makoto Matsumoto.
9428 //
9429 // Copyright (C) 1997 - 2002, Makoto Matsumoto and Takuji Nishimura,
9430 // All rights reserved.
9431 //
9432 // Redistribution and use in source and binary forms, with or without
9433 // modification, are permitted provided that the following conditions
9434 // are met:
9435 //
9436 // 1. Redistributions of source code must retain the above copyright
9437 // notice, this list of conditions and the following disclaimer.
9438 //
9439 // 2. Redistributions in binary form must reproduce the above copyright
9440 // notice, this list of conditions and the following disclaimer
9441 // in the documentation and/or other materials provided with the
9442 // distribution.
9443 //
9444 // 3. The names of its contributors may not be used to endorse or
9445 // promote products derived from this software without specific
9446 // prior written permission.
9447 //
9448 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
9449 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
9450 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
9451 // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
9452 // OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
9453 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
9454 // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
9455 // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
9456 // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
9457 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
9458 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
9459 //
9460 // ============================= END =================================
9461 do {
9462 exitg1 = 0;
9463 cartesi_genrand_uint32_vector_a(mt, cartesian_trajectory_planner_B.b_u_d);
9464 r = (static_cast<real_T>(cartesian_trajectory_planner_B.b_u_d[0] >> 5U) *
9465 6.7108864E+7 + static_cast<real_T>
9466 (cartesian_trajectory_planner_B.b_u_d[1] >> 6U)) *
9467 1.1102230246251565E-16;
9468 if (r == 0.0) {
9469 if (!cartesian_trajec_is_valid_state(mt)) {
9470 cartesian_trajectory_planner_B.r_i = 5489U;
9471 mt[0] = 5489U;
9472 for (cartesian_trajectory_planner_B.b_mti_o = 0;
9473 cartesian_trajectory_planner_B.b_mti_o < 623;
9474 cartesian_trajectory_planner_B.b_mti_o++) {
9475 cartesian_trajectory_planner_B.r_i =
9476 ((cartesian_trajectory_planner_B.r_i >> 30U ^
9477 cartesian_trajectory_planner_B.r_i) * 1812433253U +
9478 cartesian_trajectory_planner_B.b_mti_o) + 1U;
9479 mt[cartesian_trajectory_planner_B.b_mti_o + 1] =
9480 cartesian_trajectory_planner_B.r_i;
9481 }
9482
9483 mt[624] = 624U;
9484 }
9485 } else {
9486 exitg1 = 1;
9487 }
9488 } while (exitg1 == 0);
9489
9490 return r;
9491}
9492
9493static real_T cartesia_eml_rand_mt19937ar_ast(uint32_T state[625])
9494{
9495 real_T r;
9496 static const real_T tmp[257] = { 1.0, 0.977101701267673, 0.959879091800108,
9497 0.9451989534423, 0.932060075959231, 0.919991505039348, 0.908726440052131,
9498 0.898095921898344, 0.887984660755834, 0.878309655808918, 0.869008688036857,
9499 0.860033621196332, 0.851346258458678, 0.842915653112205, 0.834716292986884,
9500 0.826726833946222, 0.818929191603703, 0.811307874312656, 0.803849483170964,
9501 0.796542330422959, 0.789376143566025, 0.782341832654803, 0.775431304981187,
9502 0.768637315798486, 0.761953346836795, 0.755373506507096, 0.748892447219157,
9503 0.742505296340151, 0.736207598126863, 0.729995264561476, 0.72386453346863,
9504 0.717811932630722, 0.711834248878248, 0.705928501332754, 0.700091918136512,
9505 0.694321916126117, 0.688616083004672, 0.682972161644995, 0.677388036218774,
9506 0.671861719897082, 0.66639134390875, 0.660975147776663, 0.655611470579697,
9507 0.650298743110817, 0.645035480820822, 0.639820277453057, 0.634651799287624,
9508 0.629528779924837, 0.624450015547027, 0.619414360605834, 0.614420723888914,
9509 0.609468064925773, 0.604555390697468, 0.599681752619125, 0.594846243767987,
9510 0.590047996332826, 0.585286179263371, 0.580559996100791, 0.575868682972354,
9511 0.571211506735253, 0.566587763256165, 0.561996775814525, 0.557437893618766,
9512 0.552910490425833, 0.548413963255266, 0.543947731190026, 0.539511234256952,
9513 0.535103932380458, 0.530725304403662, 0.526374847171684, 0.522052074672322,
9514 0.517756517229756, 0.513487720747327, 0.509245245995748, 0.505028667943468,
9515 0.500837575126149, 0.49667156905249, 0.492530263643869, 0.488413284705458,
9516 0.484320269426683, 0.480250865909047, 0.476204732719506, 0.47218153846773,
9517 0.468180961405694, 0.464202689048174, 0.460246417812843, 0.456311852678716,
9518 0.452398706861849, 0.448506701507203, 0.444635565395739, 0.440785034665804,
9519 0.436954852547985, 0.433144769112652, 0.429354541029442, 0.425583931338022,
9520 0.421832709229496, 0.418100649837848, 0.414387534040891, 0.410693148270188,
9521 0.407017284329473, 0.403359739221114, 0.399720314980197, 0.396098818515832,
9522 0.392495061459315, 0.388908860018789, 0.385340034840077, 0.381788410873393,
9523 0.378253817245619, 0.374736087137891, 0.371235057668239, 0.367750569779032,
9524 0.364282468129004, 0.360830600989648, 0.357394820145781, 0.353974980800077,
9525 0.350570941481406, 0.347182563956794, 0.343809713146851, 0.340452257044522,
9526 0.337110066637006, 0.333783015830718, 0.330470981379163, 0.327173842813601,
9527 0.323891482376391, 0.320623784956905, 0.317370638029914, 0.314131931596337,
9528 0.310907558126286, 0.307697412504292, 0.30450139197665, 0.301319396100803,
9529 0.298151326696685, 0.294997087799962, 0.291856585617095, 0.288729728482183,
9530 0.285616426815502, 0.282516593083708, 0.279430141761638, 0.276356989295668,
9531 0.273297054068577, 0.270250256365875, 0.267216518343561, 0.264195763997261,
9532 0.261187919132721, 0.258192911337619, 0.255210669954662, 0.252241126055942,
9533 0.249284212418529, 0.246339863501264, 0.24340801542275, 0.240488605940501,
9534 0.237581574431238, 0.23468686187233, 0.231804410824339, 0.228934165414681,
9535 0.226076071322381, 0.223230075763918, 0.220396127480152, 0.217574176724331,
9536 0.214764175251174, 0.211966076307031, 0.209179834621125, 0.206405406397881,
9537 0.203642749310335, 0.200891822494657, 0.198152586545776, 0.195425003514135,
9538 0.192709036903589, 0.190004651670465, 0.187311814223801, 0.1846304924268,
9539 0.181960655599523, 0.179302274522848, 0.176655321443735, 0.174019770081839,
9540 0.171395595637506, 0.168782774801212, 0.166181285764482, 0.163591108232366,
9541 0.161012223437511, 0.158444614155925, 0.15588826472448, 0.153343161060263,
9542 0.150809290681846, 0.148286642732575, 0.145775208005994, 0.143274978973514,
9543 0.140785949814445, 0.138308116448551, 0.135841476571254, 0.133386029691669,
9544 0.130941777173644, 0.12850872228, 0.126086870220186, 0.123676228201597,
9545 0.12127680548479, 0.11888861344291, 0.116511665625611, 0.114145977827839,
9546 0.111791568163838, 0.109448457146812, 0.107116667774684, 0.104796225622487,
9547 0.102487158941935, 0.10018949876881, 0.0979032790388625, 0.095628536713009,
9548 0.093365311912691, 0.0911136480663738, 0.0888735920682759,
9549 0.0866451944505581, 0.0844285095703535, 0.082223595813203,
9550 0.0800305158146631, 0.0778493367020961, 0.0756801303589272,
9551 0.0735229737139814, 0.0713779490588905, 0.0692451443970068,
9552 0.0671246538277886, 0.065016577971243, 0.0629210244377582, 0.06083810834954,
9553 0.0587679529209339, 0.0567106901062031, 0.0546664613248891,
9554 0.0526354182767924, 0.0506177238609479, 0.0486135532158687,
9555 0.0466230949019305, 0.0446465522512946, 0.0426841449164746,
9556 0.0407361106559411, 0.0388027074045262, 0.0368842156885674,
9557 0.0349809414617162, 0.0330932194585786, 0.0312214171919203,
9558 0.0293659397581334, 0.0275272356696031, 0.0257058040085489,
9559 0.0239022033057959, 0.0221170627073089, 0.0203510962300445,
9560 0.0186051212757247, 0.0168800831525432, 0.0151770883079353,
9561 0.0134974506017399, 0.0118427578579079, 0.0102149714397015,
9562 0.00861658276939875, 0.00705087547137324, 0.00552240329925101,
9563 0.00403797259336304, 0.00260907274610216, 0.0012602859304986,
9564 0.000477467764609386 };
9565
9566 const real_T *fitab;
9567 int32_T exitg1;
9568 cartesian_trajectory_planner_B.xi[0] = 0.0;
9569 cartesian_trajectory_planner_B.xi[1] = 0.215241895984875;
9570 cartesian_trajectory_planner_B.xi[2] = 0.286174591792068;
9571 cartesian_trajectory_planner_B.xi[3] = 0.335737519214422;
9572 cartesian_trajectory_planner_B.xi[4] = 0.375121332878378;
9573 cartesian_trajectory_planner_B.xi[5] = 0.408389134611989;
9574 cartesian_trajectory_planner_B.xi[6] = 0.43751840220787;
9575 cartesian_trajectory_planner_B.xi[7] = 0.46363433679088;
9576 cartesian_trajectory_planner_B.xi[8] = 0.487443966139235;
9577 cartesian_trajectory_planner_B.xi[9] = 0.50942332960209;
9578 cartesian_trajectory_planner_B.xi[10] = 0.529909720661557;
9579 cartesian_trajectory_planner_B.xi[11] = 0.549151702327164;
9580 cartesian_trajectory_planner_B.xi[12] = 0.567338257053817;
9581 cartesian_trajectory_planner_B.xi[13] = 0.584616766106378;
9582 cartesian_trajectory_planner_B.xi[14] = 0.601104617755991;
9583 cartesian_trajectory_planner_B.xi[15] = 0.61689699000775;
9584 cartesian_trajectory_planner_B.xi[16] = 0.63207223638606;
9585 cartesian_trajectory_planner_B.xi[17] = 0.646695714894993;
9586 cartesian_trajectory_planner_B.xi[18] = 0.660822574244419;
9587 cartesian_trajectory_planner_B.xi[19] = 0.674499822837293;
9588 cartesian_trajectory_planner_B.xi[20] = 0.687767892795788;
9589 cartesian_trajectory_planner_B.xi[21] = 0.700661841106814;
9590 cartesian_trajectory_planner_B.xi[22] = 0.713212285190975;
9591 cartesian_trajectory_planner_B.xi[23] = 0.725446140909999;
9592 cartesian_trajectory_planner_B.xi[24] = 0.737387211434295;
9593 cartesian_trajectory_planner_B.xi[25] = 0.749056662017815;
9594 cartesian_trajectory_planner_B.xi[26] = 0.760473406430107;
9595 cartesian_trajectory_planner_B.xi[27] = 0.771654424224568;
9596 cartesian_trajectory_planner_B.xi[28] = 0.782615023307232;
9597 cartesian_trajectory_planner_B.xi[29] = 0.793369058840623;
9598 cartesian_trajectory_planner_B.xi[30] = 0.80392911698997;
9599 cartesian_trajectory_planner_B.xi[31] = 0.814306670135215;
9600 cartesian_trajectory_planner_B.xi[32] = 0.824512208752291;
9601 cartesian_trajectory_planner_B.xi[33] = 0.834555354086381;
9602 cartesian_trajectory_planner_B.xi[34] = 0.844444954909153;
9603 cartesian_trajectory_planner_B.xi[35] = 0.854189171008163;
9604 cartesian_trajectory_planner_B.xi[36] = 0.863795545553308;
9605 cartesian_trajectory_planner_B.xi[37] = 0.87327106808886;
9606 cartesian_trajectory_planner_B.xi[38] = 0.882622229585165;
9607 cartesian_trajectory_planner_B.xi[39] = 0.891855070732941;
9608 cartesian_trajectory_planner_B.xi[40] = 0.900975224461221;
9609 cartesian_trajectory_planner_B.xi[41] = 0.909987953496718;
9610 cartesian_trajectory_planner_B.xi[42] = 0.91889818364959;
9611 cartesian_trajectory_planner_B.xi[43] = 0.927710533401999;
9612 cartesian_trajectory_planner_B.xi[44] = 0.936429340286575;
9613 cartesian_trajectory_planner_B.xi[45] = 0.945058684468165;
9614 cartesian_trajectory_planner_B.xi[46] = 0.953602409881086;
9615 cartesian_trajectory_planner_B.xi[47] = 0.96206414322304;
9616 cartesian_trajectory_planner_B.xi[48] = 0.970447311064224;
9617 cartesian_trajectory_planner_B.xi[49] = 0.978755155294224;
9618 cartesian_trajectory_planner_B.xi[50] = 0.986990747099062;
9619 cartesian_trajectory_planner_B.xi[51] = 0.99515699963509;
9620 cartesian_trajectory_planner_B.xi[52] = 1.00325667954467;
9621 cartesian_trajectory_planner_B.xi[53] = 1.01129241744;
9622 cartesian_trajectory_planner_B.xi[54] = 1.01926671746548;
9623 cartesian_trajectory_planner_B.xi[55] = 1.02718196603564;
9624 cartesian_trajectory_planner_B.xi[56] = 1.03504043983344;
9625 cartesian_trajectory_planner_B.xi[57] = 1.04284431314415;
9626 cartesian_trajectory_planner_B.xi[58] = 1.05059566459093;
9627 cartesian_trajectory_planner_B.xi[59] = 1.05829648333067;
9628 cartesian_trajectory_planner_B.xi[60] = 1.06594867476212;
9629 cartesian_trajectory_planner_B.xi[61] = 1.07355406579244;
9630 cartesian_trajectory_planner_B.xi[62] = 1.0811144097034;
9631 cartesian_trajectory_planner_B.xi[63] = 1.08863139065398;
9632 cartesian_trajectory_planner_B.xi[64] = 1.09610662785202;
9633 cartesian_trajectory_planner_B.xi[65] = 1.10354167942464;
9634 cartesian_trajectory_planner_B.xi[66] = 1.11093804601357;
9635 cartesian_trajectory_planner_B.xi[67] = 1.11829717411934;
9636 cartesian_trajectory_planner_B.xi[68] = 1.12562045921553;
9637 cartesian_trajectory_planner_B.xi[69] = 1.13290924865253;
9638 cartesian_trajectory_planner_B.xi[70] = 1.14016484436815;
9639 cartesian_trajectory_planner_B.xi[71] = 1.14738850542085;
9640 cartesian_trajectory_planner_B.xi[72] = 1.15458145035993;
9641 cartesian_trajectory_planner_B.xi[73] = 1.16174485944561;
9642 cartesian_trajectory_planner_B.xi[74] = 1.16887987673083;
9643 cartesian_trajectory_planner_B.xi[75] = 1.17598761201545;
9644 cartesian_trajectory_planner_B.xi[76] = 1.18306914268269;
9645 cartesian_trajectory_planner_B.xi[77] = 1.19012551542669;
9646 cartesian_trajectory_planner_B.xi[78] = 1.19715774787944;
9647 cartesian_trajectory_planner_B.xi[79] = 1.20416683014438;
9648 cartesian_trajectory_planner_B.xi[80] = 1.2111537262437;
9649 cartesian_trajectory_planner_B.xi[81] = 1.21811937548548;
9650 cartesian_trajectory_planner_B.xi[82] = 1.22506469375653;
9651 cartesian_trajectory_planner_B.xi[83] = 1.23199057474614;
9652 cartesian_trajectory_planner_B.xi[84] = 1.23889789110569;
9653 cartesian_trajectory_planner_B.xi[85] = 1.24578749554863;
9654 cartesian_trajectory_planner_B.xi[86] = 1.2526602218949;
9655 cartesian_trajectory_planner_B.xi[87] = 1.25951688606371;
9656 cartesian_trajectory_planner_B.xi[88] = 1.26635828701823;
9657 cartesian_trajectory_planner_B.xi[89] = 1.27318520766536;
9658 cartesian_trajectory_planner_B.xi[90] = 1.27999841571382;
9659 cartesian_trajectory_planner_B.xi[91] = 1.28679866449324;
9660 cartesian_trajectory_planner_B.xi[92] = 1.29358669373695;
9661 cartesian_trajectory_planner_B.xi[93] = 1.30036323033084;
9662 cartesian_trajectory_planner_B.xi[94] = 1.30712898903073;
9663 cartesian_trajectory_planner_B.xi[95] = 1.31388467315022;
9664 cartesian_trajectory_planner_B.xi[96] = 1.32063097522106;
9665 cartesian_trajectory_planner_B.xi[97] = 1.32736857762793;
9666 cartesian_trajectory_planner_B.xi[98] = 1.33409815321936;
9667 cartesian_trajectory_planner_B.xi[99] = 1.3408203658964;
9668 cartesian_trajectory_planner_B.xi[100] = 1.34753587118059;
9669 cartesian_trajectory_planner_B.xi[101] = 1.35424531676263;
9670 cartesian_trajectory_planner_B.xi[102] = 1.36094934303328;
9671 cartesian_trajectory_planner_B.xi[103] = 1.36764858359748;
9672 cartesian_trajectory_planner_B.xi[104] = 1.37434366577317;
9673 cartesian_trajectory_planner_B.xi[105] = 1.38103521107586;
9674 cartesian_trajectory_planner_B.xi[106] = 1.38772383568998;
9675 cartesian_trajectory_planner_B.xi[107] = 1.39441015092814;
9676 cartesian_trajectory_planner_B.xi[108] = 1.40109476367925;
9677 cartesian_trajectory_planner_B.xi[109] = 1.4077782768464;
9678 cartesian_trajectory_planner_B.xi[110] = 1.41446128977547;
9679 cartesian_trajectory_planner_B.xi[111] = 1.42114439867531;
9680 cartesian_trajectory_planner_B.xi[112] = 1.42782819703026;
9681 cartesian_trajectory_planner_B.xi[113] = 1.43451327600589;
9682 cartesian_trajectory_planner_B.xi[114] = 1.44120022484872;
9683 cartesian_trajectory_planner_B.xi[115] = 1.44788963128058;
9684 cartesian_trajectory_planner_B.xi[116] = 1.45458208188841;
9685 cartesian_trajectory_planner_B.xi[117] = 1.46127816251028;
9686 cartesian_trajectory_planner_B.xi[118] = 1.46797845861808;
9687 cartesian_trajectory_planner_B.xi[119] = 1.47468355569786;
9688 cartesian_trajectory_planner_B.xi[120] = 1.48139403962819;
9689 cartesian_trajectory_planner_B.xi[121] = 1.48811049705745;
9690 cartesian_trajectory_planner_B.xi[122] = 1.49483351578049;
9691 cartesian_trajectory_planner_B.xi[123] = 1.50156368511546;
9692 cartesian_trajectory_planner_B.xi[124] = 1.50830159628131;
9693 cartesian_trajectory_planner_B.xi[125] = 1.51504784277671;
9694 cartesian_trajectory_planner_B.xi[126] = 1.521803020761;
9695 cartesian_trajectory_planner_B.xi[127] = 1.52856772943771;
9696 cartesian_trajectory_planner_B.xi[128] = 1.53534257144151;
9697 cartesian_trajectory_planner_B.xi[129] = 1.542128153229;
9698 cartesian_trajectory_planner_B.xi[130] = 1.54892508547417;
9699 cartesian_trajectory_planner_B.xi[131] = 1.55573398346918;
9700 cartesian_trajectory_planner_B.xi[132] = 1.56255546753104;
9701 cartesian_trajectory_planner_B.xi[133] = 1.56939016341512;
9702 cartesian_trajectory_planner_B.xi[134] = 1.57623870273591;
9703 cartesian_trajectory_planner_B.xi[135] = 1.58310172339603;
9704 cartesian_trajectory_planner_B.xi[136] = 1.58997987002419;
9705 cartesian_trajectory_planner_B.xi[137] = 1.59687379442279;
9706 cartesian_trajectory_planner_B.xi[138] = 1.60378415602609;
9707 cartesian_trajectory_planner_B.xi[139] = 1.61071162236983;
9708 cartesian_trajectory_planner_B.xi[140] = 1.61765686957301;
9709 cartesian_trajectory_planner_B.xi[141] = 1.62462058283303;
9710 cartesian_trajectory_planner_B.xi[142] = 1.63160345693487;
9711 cartesian_trajectory_planner_B.xi[143] = 1.63860619677555;
9712 cartesian_trajectory_planner_B.xi[144] = 1.64562951790478;
9713 cartesian_trajectory_planner_B.xi[145] = 1.65267414708306;
9714 cartesian_trajectory_planner_B.xi[146] = 1.65974082285818;
9715 cartesian_trajectory_planner_B.xi[147] = 1.66683029616166;
9716 cartesian_trajectory_planner_B.xi[148] = 1.67394333092612;
9717 cartesian_trajectory_planner_B.xi[149] = 1.68108070472517;
9718 cartesian_trajectory_planner_B.xi[150] = 1.68824320943719;
9719 cartesian_trajectory_planner_B.xi[151] = 1.69543165193456;
9720 cartesian_trajectory_planner_B.xi[152] = 1.70264685479992;
9721 cartesian_trajectory_planner_B.xi[153] = 1.7098896570713;
9722 cartesian_trajectory_planner_B.xi[154] = 1.71716091501782;
9723 cartesian_trajectory_planner_B.xi[155] = 1.72446150294804;
9724 cartesian_trajectory_planner_B.xi[156] = 1.73179231405296;
9725 cartesian_trajectory_planner_B.xi[157] = 1.73915426128591;
9726 cartesian_trajectory_planner_B.xi[158] = 1.74654827828172;
9727 cartesian_trajectory_planner_B.xi[159] = 1.75397532031767;
9728 cartesian_trajectory_planner_B.xi[160] = 1.76143636531891;
9729 cartesian_trajectory_planner_B.xi[161] = 1.76893241491127;
9730 cartesian_trajectory_planner_B.xi[162] = 1.77646449552452;
9731 cartesian_trajectory_planner_B.xi[163] = 1.78403365954944;
9732 cartesian_trajectory_planner_B.xi[164] = 1.79164098655216;
9733 cartesian_trajectory_planner_B.xi[165] = 1.79928758454972;
9734 cartesian_trajectory_planner_B.xi[166] = 1.80697459135082;
9735 cartesian_trajectory_planner_B.xi[167] = 1.81470317596628;
9736 cartesian_trajectory_planner_B.xi[168] = 1.82247454009388;
9737 cartesian_trajectory_planner_B.xi[169] = 1.83028991968276;
9738 cartesian_trajectory_planner_B.xi[170] = 1.83815058658281;
9739 cartesian_trajectory_planner_B.xi[171] = 1.84605785028518;
9740 cartesian_trajectory_planner_B.xi[172] = 1.8540130597602;
9741 cartesian_trajectory_planner_B.xi[173] = 1.86201760539967;
9742 cartesian_trajectory_planner_B.xi[174] = 1.87007292107127;
9743 cartesian_trajectory_planner_B.xi[175] = 1.878180486293;
9744 cartesian_trajectory_planner_B.xi[176] = 1.88634182853678;
9745 cartesian_trajectory_planner_B.xi[177] = 1.8945585256707;
9746 cartesian_trajectory_planner_B.xi[178] = 1.90283220855043;
9747 cartesian_trajectory_planner_B.xi[179] = 1.91116456377125;
9748 cartesian_trajectory_planner_B.xi[180] = 1.91955733659319;
9749 cartesian_trajectory_planner_B.xi[181] = 1.92801233405266;
9750 cartesian_trajectory_planner_B.xi[182] = 1.93653142827569;
9751 cartesian_trajectory_planner_B.xi[183] = 1.94511656000868;
9752 cartesian_trajectory_planner_B.xi[184] = 1.95376974238465;
9753 cartesian_trajectory_planner_B.xi[185] = 1.96249306494436;
9754 cartesian_trajectory_planner_B.xi[186] = 1.97128869793366;
9755 cartesian_trajectory_planner_B.xi[187] = 1.98015889690048;
9756 cartesian_trajectory_planner_B.xi[188] = 1.98910600761744;
9757 cartesian_trajectory_planner_B.xi[189] = 1.99813247135842;
9758 cartesian_trajectory_planner_B.xi[190] = 2.00724083056053;
9759 cartesian_trajectory_planner_B.xi[191] = 2.0164337349062;
9760 cartesian_trajectory_planner_B.xi[192] = 2.02571394786385;
9761 cartesian_trajectory_planner_B.xi[193] = 2.03508435372962;
9762 cartesian_trajectory_planner_B.xi[194] = 2.04454796521753;
9763 cartesian_trajectory_planner_B.xi[195] = 2.05410793165065;
9764 cartesian_trajectory_planner_B.xi[196] = 2.06376754781173;
9765 cartesian_trajectory_planner_B.xi[197] = 2.07353026351874;
9766 cartesian_trajectory_planner_B.xi[198] = 2.0833996939983;
9767 cartesian_trajectory_planner_B.xi[199] = 2.09337963113879;
9768 cartesian_trajectory_planner_B.xi[200] = 2.10347405571488;
9769 cartesian_trajectory_planner_B.xi[201] = 2.11368715068665;
9770 cartesian_trajectory_planner_B.xi[202] = 2.12402331568952;
9771 cartesian_trajectory_planner_B.xi[203] = 2.13448718284602;
9772 cartesian_trajectory_planner_B.xi[204] = 2.14508363404789;
9773 cartesian_trajectory_planner_B.xi[205] = 2.15581781987674;
9774 cartesian_trajectory_planner_B.xi[206] = 2.16669518035431;
9775 cartesian_trajectory_planner_B.xi[207] = 2.17772146774029;
9776 cartesian_trajectory_planner_B.xi[208] = 2.18890277162636;
9777 cartesian_trajectory_planner_B.xi[209] = 2.20024554661128;
9778 cartesian_trajectory_planner_B.xi[210] = 2.21175664288416;
9779 cartesian_trajectory_planner_B.xi[211] = 2.22344334009251;
9780 cartesian_trajectory_planner_B.xi[212] = 2.23531338492992;
9781 cartesian_trajectory_planner_B.xi[213] = 2.24737503294739;
9782 cartesian_trajectory_planner_B.xi[214] = 2.25963709517379;
9783 cartesian_trajectory_planner_B.xi[215] = 2.27210899022838;
9784 cartesian_trajectory_planner_B.xi[216] = 2.28480080272449;
9785 cartesian_trajectory_planner_B.xi[217] = 2.29772334890286;
9786 cartesian_trajectory_planner_B.xi[218] = 2.31088825060137;
9787 cartesian_trajectory_planner_B.xi[219] = 2.32430801887113;
9788 cartesian_trajectory_planner_B.xi[220] = 2.33799614879653;
9789 cartesian_trajectory_planner_B.xi[221] = 2.35196722737914;
9790 cartesian_trajectory_planner_B.xi[222] = 2.36623705671729;
9791 cartesian_trajectory_planner_B.xi[223] = 2.38082279517208;
9792 cartesian_trajectory_planner_B.xi[224] = 2.39574311978193;
9793 cartesian_trajectory_planner_B.xi[225] = 2.41101841390112;
9794 cartesian_trajectory_planner_B.xi[226] = 2.42667098493715;
9795 cartesian_trajectory_planner_B.xi[227] = 2.44272531820036;
9796 cartesian_trajectory_planner_B.xi[228] = 2.4592083743347;
9797 cartesian_trajectory_planner_B.xi[229] = 2.47614993967052;
9798 cartesian_trajectory_planner_B.xi[230] = 2.49358304127105;
9799 cartesian_trajectory_planner_B.xi[231] = 2.51154444162669;
9800 cartesian_trajectory_planner_B.xi[232] = 2.53007523215985;
9801 cartesian_trajectory_planner_B.xi[233] = 2.54922155032478;
9802 cartesian_trajectory_planner_B.xi[234] = 2.56903545268184;
9803 cartesian_trajectory_planner_B.xi[235] = 2.58957598670829;
9804 cartesian_trajectory_planner_B.xi[236] = 2.61091051848882;
9805 cartesian_trajectory_planner_B.xi[237] = 2.63311639363158;
9806 cartesian_trajectory_planner_B.xi[238] = 2.65628303757674;
9807 cartesian_trajectory_planner_B.xi[239] = 2.68051464328574;
9808 cartesian_trajectory_planner_B.xi[240] = 2.70593365612306;
9809 cartesian_trajectory_planner_B.xi[241] = 2.73268535904401;
9810 cartesian_trajectory_planner_B.xi[242] = 2.76094400527999;
9811 cartesian_trajectory_planner_B.xi[243] = 2.79092117400193;
9812 cartesian_trajectory_planner_B.xi[244] = 2.82287739682644;
9813 cartesian_trajectory_planner_B.xi[245] = 2.85713873087322;
9814 cartesian_trajectory_planner_B.xi[246] = 2.89412105361341;
9815 cartesian_trajectory_planner_B.xi[247] = 2.93436686720889;
9816 cartesian_trajectory_planner_B.xi[248] = 2.97860327988184;
9817 cartesian_trajectory_planner_B.xi[249] = 3.02783779176959;
9818 cartesian_trajectory_planner_B.xi[250] = 3.08352613200214;
9819 cartesian_trajectory_planner_B.xi[251] = 3.147889289518;
9820 cartesian_trajectory_planner_B.xi[252] = 3.2245750520478;
9821 cartesian_trajectory_planner_B.xi[253] = 3.32024473383983;
9822 cartesian_trajectory_planner_B.xi[254] = 3.44927829856143;
9823 cartesian_trajectory_planner_B.xi[255] = 3.65415288536101;
9824 cartesian_trajectory_planner_B.xi[256] = 3.91075795952492;
9825 fitab = &tmp[0];
9826 do {
9827 exitg1 = 0;
9828 cartesi_genrand_uint32_vector_a(state, cartesian_trajectory_planner_B.u32);
9829 cartesian_trajectory_planner_B.i_g = static_cast<int32_T>
9830 ((cartesian_trajectory_planner_B.u32[1] >> 24U) + 1U);
9831 r = ((static_cast<real_T>(cartesian_trajectory_planner_B.u32[0] >> 3U) *
9832 1.6777216E+7 + static_cast<real_T>(static_cast<int32_T>
9833 (cartesian_trajectory_planner_B.u32[1]) & 16777215)) *
9834 2.2204460492503131E-16 - 1.0) *
9835 cartesian_trajectory_planner_B.xi[cartesian_trajectory_planner_B.i_g];
9836 if (fabs(r) <=
9837 cartesian_trajectory_planner_B.xi[cartesian_trajectory_planner_B.i_g - 1])
9838 {
9839 exitg1 = 1;
9840 } else if (cartesian_trajectory_planner_B.i_g < 256) {
9841 cartesian_trajectory_planner_B.x_h = cartesian_trajectory_genrandu_a(state);
9842 if ((fitab[cartesian_trajectory_planner_B.i_g - 1] -
9843 fitab[cartesian_trajectory_planner_B.i_g]) *
9844 cartesian_trajectory_planner_B.x_h +
9845 fitab[cartesian_trajectory_planner_B.i_g] < exp(-0.5 * r * r)) {
9846 exitg1 = 1;
9847 }
9848 } else {
9849 do {
9850 cartesian_trajectory_planner_B.x_h = cartesian_trajectory_genrandu_a
9851 (state);
9852 cartesian_trajectory_planner_B.x_h = log
9853 (cartesian_trajectory_planner_B.x_h) * 0.273661237329758;
9854 cartesian_trajectory_planner_B.d_u = cartesian_trajectory_genrandu_a
9855 (state);
9856 } while (!(-2.0 * log(cartesian_trajectory_planner_B.d_u) >
9857 cartesian_trajectory_planner_B.x_h *
9858 cartesian_trajectory_planner_B.x_h));
9859
9860 if (r < 0.0) {
9861 r = cartesian_trajectory_planner_B.x_h - 3.65415288536101;
9862 } else {
9863 r = 3.65415288536101 - cartesian_trajectory_planner_B.x_h;
9864 }
9865
9866 exitg1 = 1;
9867 }
9868 } while (exitg1 == 0);
9869
9870 return r;
9871}
9872
9873static void cartesian_trajectory_plan_randn(const real_T varargin_1[2],
9874 emxArray_real_T_cartesian_tra_T *r)
9875{
9876 cartesian_trajectory_planner_B.b_k_j = r->size[0] * r->size[1];
9877 r->size[0] = static_cast<int32_T>(varargin_1[0]);
9878 r->size[1] = 1;
9879 cartes_emxEnsureCapacity_real_T(r, cartesian_trajectory_planner_B.b_k_j);
9880 cartesian_trajectory_planner_B.d_j = r->size[0] - 1;
9881 for (cartesian_trajectory_planner_B.b_k_j = 0;
9882 cartesian_trajectory_planner_B.b_k_j <=
9883 cartesian_trajectory_planner_B.d_j; cartesian_trajectory_planner_B.b_k_j
9884 ++) {
9885 r->data[cartesian_trajectory_planner_B.b_k_j] =
9886 cartesia_eml_rand_mt19937ar_ast(cartesian_trajectory_planner_DW.state_b);
9887 }
9888}
9889
9890static void cartesian__eml_rand_mt19937ar_a(const uint32_T state[625], uint32_T
9891 b_state[625], real_T *r)
9892{
9893 int32_T exitg1;
9894 memcpy(&b_state[0], &state[0], 625U * sizeof(uint32_T));
9895
9896 // ========================= COPYRIGHT NOTICE ============================
9897 // This is a uniform (0,1) pseudorandom number generator based on:
9898 //
9899 // A C-program for MT19937, with initialization improved 2002/1/26.
9900 // Coded by Takuji Nishimura and Makoto Matsumoto.
9901 //
9902 // Copyright (C) 1997 - 2002, Makoto Matsumoto and Takuji Nishimura,
9903 // All rights reserved.
9904 //
9905 // Redistribution and use in source and binary forms, with or without
9906 // modification, are permitted provided that the following conditions
9907 // are met:
9908 //
9909 // 1. Redistributions of source code must retain the above copyright
9910 // notice, this list of conditions and the following disclaimer.
9911 //
9912 // 2. Redistributions in binary form must reproduce the above copyright
9913 // notice, this list of conditions and the following disclaimer
9914 // in the documentation and/or other materials provided with the
9915 // distribution.
9916 //
9917 // 3. The names of its contributors may not be used to endorse or
9918 // promote products derived from this software without specific
9919 // prior written permission.
9920 //
9921 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
9922 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
9923 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
9924 // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
9925 // OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
9926 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
9927 // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
9928 // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
9929 // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
9930 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
9931 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
9932 //
9933 // ============================= END =================================
9934 do {
9935 exitg1 = 0;
9936 cartesi_genrand_uint32_vector_a(b_state, cartesian_trajectory_planner_B.b_u);
9937 *r = (static_cast<real_T>(cartesian_trajectory_planner_B.b_u[0] >> 5U) *
9938 6.7108864E+7 + static_cast<real_T>(cartesian_trajectory_planner_B.b_u
9939 [1] >> 6U)) * 1.1102230246251565E-16;
9940 if (*r == 0.0) {
9941 if (!cartesian_trajec_is_valid_state(b_state)) {
9942 cartesian_trajectory_planner_B.r = 5489U;
9943 b_state[0] = 5489U;
9944 for (cartesian_trajectory_planner_B.b_mti = 0;
9945 cartesian_trajectory_planner_B.b_mti < 623;
9946 cartesian_trajectory_planner_B.b_mti++) {
9947 cartesian_trajectory_planner_B.r = ((cartesian_trajectory_planner_B.r >>
9948 30U ^ cartesian_trajectory_planner_B.r) * 1812433253U +
9949 cartesian_trajectory_planner_B.b_mti) + 1U;
9950 b_state[cartesian_trajectory_planner_B.b_mti + 1] =
9951 cartesian_trajectory_planner_B.r;
9952 }
9953
9954 b_state[624] = 624U;
9955 }
9956 } else {
9957 exitg1 = 1;
9958 }
9959 } while (exitg1 == 0);
9960}
9961
9962static void cartesian_trajectory_pla_rand_a(real_T varargin_1,
9963 emxArray_real_T_cartesian_tra_T *r)
9964{
9965 cartesian_trajectory_planner_B.b_k_g = r->size[0];
9966 cartesian_trajectory_planner_B.d = static_cast<int32_T>(varargin_1);
9967 r->size[0] = cartesian_trajectory_planner_B.d;
9968 cartes_emxEnsureCapacity_real_T(r, cartesian_trajectory_planner_B.b_k_g);
9969 cartesian_trajectory_planner_B.d--;
9970 for (cartesian_trajectory_planner_B.b_k_g = 0;
9971 cartesian_trajectory_planner_B.b_k_g <= cartesian_trajectory_planner_B.d;
9972 cartesian_trajectory_planner_B.b_k_g++) {
9973 memcpy(&cartesian_trajectory_planner_B.uv1[0],
9974 &cartesian_trajectory_planner_DW.state_b[0], 625U * sizeof(uint32_T));
9975 cartesian__eml_rand_mt19937ar_a(cartesian_trajectory_planner_B.uv1,
9976 cartesian_trajectory_planner_DW.state_b, &r->
9977 data[cartesian_trajectory_planner_B.b_k_g]);
9978 }
9979}
9980
9981static void cartes_NLPSolverInterface_solve(h_robotics_core_internal_Damp_T *obj,
9982 const real_T seed[6], real_T xSol[6], real_T *solutionInfo_Iterations, real_T *
9983 solutionInfo_RRAttempts, real_T *solutionInfo_Error, real_T
9984 *solutionInfo_ExitFlag, char_T solutionInfo_Status_data[], int32_T
9985 solutionInfo_Status_size[2])
9986{
9987 emxArray_real_T_cartesian_tra_T *newseed;
9988 f_robotics_manip_internal_IKE_T *args;
9989 x_robotics_manip_internal_Rig_T *obj_0;
9990 emxArray_real_T_cartesian_tra_T *qi;
9991 c_rigidBodyJoint_cartesian__a_T *obj_1;
9992 emxArray_real_T_cartesian_tra_T *ub;
9993 emxArray_real_T_cartesian_tra_T *lb;
9994 emxArray_real_T_cartesian_tra_T *rn;
9995 emxArray_real_T_cartesian_tra_T *e;
9996 emxArray_boolean_T_cartesian__T *x;
9997 emxArray_boolean_T_cartesian__T *x_tmp;
9998 emxArray_boolean_T_cartesian__T *x_tmp_0;
9999 emxArray_boolean_T_cartesian__T *x_0;
10000 static const char_T tmp[14] = { 'b', 'e', 's', 't', ' ', 'a', 'v', 'a', 'i',
10001 'l', 'a', 'b', 'l', 'e' };
10002
10003 static const char_T tmp_0[7] = { 's', 'u', 'c', 'c', 'e', 's', 's' };
10004
10005 boolean_T guard1 = false;
10006 boolean_T guard2 = false;
10007 boolean_T guard3 = false;
10008 boolean_T exitg1;
10009 boolean_T exitg2;
10010 obj->MaxNumIterationInternal = obj->MaxNumIteration;
10011 obj->MaxTimeInternal = obj->MaxTime;
10012 for (cartesian_trajectory_planner_B.i_h = 0;
10013 cartesian_trajectory_planner_B.i_h < 6;
10014 cartesian_trajectory_planner_B.i_h++) {
10015 obj->SeedInternal[cartesian_trajectory_planner_B.i_h] =
10016 seed[cartesian_trajectory_planner_B.i_h];
10017 }
10018
10019 cartesian_trajectory_planner_B.tol = obj->SolutionTolerance;
10020 obj->TimeObj.StartTime = ctimefun();
10021 DampedBFGSwGradientProjection_s(obj, xSol,
10022 &cartesian_trajectory_planner_B.exitFlag,
10023 &cartesian_trajectory_planner_B.err, &cartesian_trajectory_planner_B.iter);
10024 *solutionInfo_RRAttempts = 0.0;
10025 *solutionInfo_Iterations = cartesian_trajectory_planner_B.iter;
10026 *solutionInfo_Error = cartesian_trajectory_planner_B.err;
10027 cartesian_trajectory_planner_B.exitFlagPrev =
10028 cartesian_trajectory_planner_B.exitFlag;
10029 cartesian_trajec_emxInit_real_T(&newseed, 1);
10030 cartesian_trajec_emxInit_real_T(&qi, 2);
10031 cartesian_trajec_emxInit_real_T(&ub, 1);
10032 cartesian_trajec_emxInit_real_T(&lb, 1);
10033 cartesian_trajec_emxInit_real_T(&rn, 1);
10034 cartesian_trajec_emxInit_real_T(&e, 2);
10035 cartesian_tra_emxInit_boolean_T(&x, 1);
10036 cartesian_tra_emxInit_boolean_T(&x_tmp, 1);
10037 cartesian_tra_emxInit_boolean_T(&x_tmp_0, 1);
10038 cartesian_tra_emxInit_boolean_T(&x_0, 1);
10039 exitg1 = false;
10040 while ((!exitg1) && (obj->RandomRestart && (cartesian_trajectory_planner_B.err
10041 > cartesian_trajectory_planner_B.tol))) {
10042 obj->MaxNumIterationInternal -= cartesian_trajectory_planner_B.iter;
10043 cartesian_trajectory_planner_B.err = ctimefun();
10044 cartesian_trajectory_planner_B.err -= obj->TimeObj.StartTime;
10045 obj->MaxTimeInternal = obj->MaxTime - cartesian_trajectory_planner_B.err;
10046 if (obj->MaxNumIterationInternal <= 0.0) {
10047 cartesian_trajectory_planner_B.exitFlag = IterationLimitExceeded;
10048 }
10049
10050 if ((cartesian_trajectory_planner_B.exitFlag == IterationLimitExceeded) ||
10051 (cartesian_trajectory_planner_B.exitFlag == TimeLimitExceeded)) {
10052 cartesian_trajectory_planner_B.exitFlagPrev =
10053 cartesian_trajectory_planner_B.exitFlag;
10054 exitg1 = true;
10055 } else {
10056 args = obj->ExtraArgs;
10057 obj_0 = args->Robot;
10058 cartesian_trajectory_planner_B.ix = newseed->size[0];
10059 newseed->size[0] = static_cast<int32_T>(obj_0->PositionNumber);
10060 cartes_emxEnsureCapacity_real_T(newseed, cartesian_trajectory_planner_B.ix);
10061 cartesian_trajectory_planner_B.nx = static_cast<int32_T>
10062 (obj_0->PositionNumber);
10063 for (cartesian_trajectory_planner_B.ix = 0;
10064 cartesian_trajectory_planner_B.ix < cartesian_trajectory_planner_B.nx;
10065 cartesian_trajectory_planner_B.ix++) {
10066 newseed->data[cartesian_trajectory_planner_B.ix] = 0.0;
10067 }
10068
10069 cartesian_trajectory_planner_B.err = obj_0->NumBodies;
10070 cartesian_trajectory_planner_B.c_c = static_cast<int32_T>
10071 (cartesian_trajectory_planner_B.err) - 1;
10072 for (cartesian_trajectory_planner_B.i_h = 0;
10073 cartesian_trajectory_planner_B.i_h <=
10074 cartesian_trajectory_planner_B.c_c;
10075 cartesian_trajectory_planner_B.i_h++) {
10076 cartesian_trajectory_planner_B.err = obj_0->
10077 PositionDoFMap[cartesian_trajectory_planner_B.i_h];
10078 cartesian_trajectory_planner_B.iter = obj_0->
10079 PositionDoFMap[cartesian_trajectory_planner_B.i_h + 8];
10080 if (cartesian_trajectory_planner_B.err <=
10081 cartesian_trajectory_planner_B.iter) {
10082 obj_1 = obj_0->Bodies[cartesian_trajectory_planner_B.i_h]
10083 ->JointInternal;
10084 if (static_cast<int32_T>(obj_1->PositionNumber) == 0) {
10085 cartesian_trajectory_planner_B.ix = qi->size[0] * qi->size[1];
10086 qi->size[0] = 1;
10087 qi->size[1] = 1;
10088 cartes_emxEnsureCapacity_real_T(qi,
10089 cartesian_trajectory_planner_B.ix);
10090 qi->data[0] = (rtNaN);
10091 } else {
10092 cartesian_trajectory_planner_B.nx = obj_1->
10093 PositionLimitsInternal->size[0];
10094 cartesian_trajectory_planner_B.ix = ub->size[0];
10095 ub->size[0] = cartesian_trajectory_planner_B.nx;
10096 cartes_emxEnsureCapacity_real_T(ub,
10097 cartesian_trajectory_planner_B.ix);
10098 for (cartesian_trajectory_planner_B.ix = 0;
10099 cartesian_trajectory_planner_B.ix <
10100 cartesian_trajectory_planner_B.nx;
10101 cartesian_trajectory_planner_B.ix++) {
10102 ub->data[cartesian_trajectory_planner_B.ix] =
10103 obj_1->PositionLimitsInternal->
10104 data[cartesian_trajectory_planner_B.ix +
10105 obj_1->PositionLimitsInternal->size[0]];
10106 }
10107
10108 cartesian_trajectory_planner_B.nx = obj_1->
10109 PositionLimitsInternal->size[0];
10110 cartesian_trajectory_planner_B.ix = lb->size[0];
10111 lb->size[0] = cartesian_trajectory_planner_B.nx;
10112 cartes_emxEnsureCapacity_real_T(lb,
10113 cartesian_trajectory_planner_B.ix);
10114 for (cartesian_trajectory_planner_B.ix = 0;
10115 cartesian_trajectory_planner_B.ix <
10116 cartesian_trajectory_planner_B.nx;
10117 cartesian_trajectory_planner_B.ix++) {
10118 lb->data[cartesian_trajectory_planner_B.ix] =
10119 obj_1->PositionLimitsInternal->
10120 data[cartesian_trajectory_planner_B.ix];
10121 }
10122
10123 cartesian_trajectory_p_isfinite(lb, x_tmp);
10124 cartesian_trajectory_planner_B.y_eb = true;
10125 cartesian_trajectory_planner_B.ix = 0;
10126 exitg2 = false;
10127 while ((!exitg2) && (cartesian_trajectory_planner_B.ix + 1 <=
10128 x_tmp->size[0])) {
10129 if (!x_tmp->data[cartesian_trajectory_planner_B.ix]) {
10130 cartesian_trajectory_planner_B.y_eb = false;
10131 exitg2 = true;
10132 } else {
10133 cartesian_trajectory_planner_B.ix++;
10134 }
10135 }
10136
10137 guard1 = false;
10138 guard2 = false;
10139 guard3 = false;
10140 if (cartesian_trajectory_planner_B.y_eb) {
10141 cartesian_trajectory_p_isfinite(ub, x);
10142 cartesian_trajectory_planner_B.y_eb = true;
10143 cartesian_trajectory_planner_B.ix = 0;
10144 exitg2 = false;
10145 while ((!exitg2) && (cartesian_trajectory_planner_B.ix + 1 <=
10146 x->size[0])) {
10147 if (!x->data[cartesian_trajectory_planner_B.ix]) {
10148 cartesian_trajectory_planner_B.y_eb = false;
10149 exitg2 = true;
10150 } else {
10151 cartesian_trajectory_planner_B.ix++;
10152 }
10153 }
10154
10155 if (cartesian_trajectory_planner_B.y_eb) {
10156 cartesian_trajectory_pla_rand_a(obj_1->PositionNumber, rn);
10157 cartesian_trajectory_planner_B.ix = qi->size[0] * qi->size[1];
10158 qi->size[0] = lb->size[0];
10159 qi->size[1] = 1;
10160 cartes_emxEnsureCapacity_real_T(qi,
10161 cartesian_trajectory_planner_B.ix);
10162 cartesian_trajectory_planner_B.nx = lb->size[0] - 1;
10163 for (cartesian_trajectory_planner_B.ix = 0;
10164 cartesian_trajectory_planner_B.ix <=
10165 cartesian_trajectory_planner_B.nx;
10166 cartesian_trajectory_planner_B.ix++) {
10167 qi->data[cartesian_trajectory_planner_B.ix] = (ub->
10168 data[cartesian_trajectory_planner_B.ix] - lb->
10169 data[cartesian_trajectory_planner_B.ix]) * rn->
10170 data[cartesian_trajectory_planner_B.ix] + lb->
10171 data[cartesian_trajectory_planner_B.ix];
10172 }
10173 } else {
10174 guard3 = true;
10175 }
10176 } else {
10177 guard3 = true;
10178 }
10179
10180 if (guard3) {
10181 cartesian_trajectory_planner_B.y_eb = true;
10182 cartesian_trajectory_planner_B.ix = 0;
10183 exitg2 = false;
10184 while ((!exitg2) && (cartesian_trajectory_planner_B.ix + 1 <=
10185 x_tmp->size[0])) {
10186 if (!x_tmp->data[cartesian_trajectory_planner_B.ix]) {
10187 cartesian_trajectory_planner_B.y_eb = false;
10188 exitg2 = true;
10189 } else {
10190 cartesian_trajectory_planner_B.ix++;
10191 }
10192 }
10193
10194 if (cartesian_trajectory_planner_B.y_eb) {
10195 cartesian_trajectory_p_isfinite(ub, x);
10196 cartesian_trajectory_planner_B.ix = x_0->size[0];
10197 x_0->size[0] = x->size[0];
10198 car_emxEnsureCapacity_boolean_T(x_0,
10199 cartesian_trajectory_planner_B.ix);
10200 cartesian_trajectory_planner_B.nx = x->size[0];
10201 for (cartesian_trajectory_planner_B.ix = 0;
10202 cartesian_trajectory_planner_B.ix <
10203 cartesian_trajectory_planner_B.nx;
10204 cartesian_trajectory_planner_B.ix++) {
10205 x_0->data[cartesian_trajectory_planner_B.ix] = !x->
10206 data[cartesian_trajectory_planner_B.ix];
10207 }
10208
10209 if (cartesian_trajectory_planne_any(x_0)) {
10210 cartesian_trajectory_planner_B.ub[0] = lb->size[0];
10211 cartesian_trajectory_planner_B.ub[1] = 1.0;
10212 cartesian_trajectory_plan_randn
10213 (cartesian_trajectory_planner_B.ub, qi);
10214 cartesian_trajectory_planner_B.nx = qi->size[0] - 1;
10215 cartesian_trajectory_planner_B.ix = e->size[0] * e->size[1];
10216 e->size[0] = qi->size[0];
10217 e->size[1] = 1;
10218 cartes_emxEnsureCapacity_real_T(e,
10219 cartesian_trajectory_planner_B.ix);
10220 for (cartesian_trajectory_planner_B.ix = 0;
10221 cartesian_trajectory_planner_B.ix <=
10222 cartesian_trajectory_planner_B.nx;
10223 cartesian_trajectory_planner_B.ix++) {
10224 e->data[cartesian_trajectory_planner_B.ix] = fabs(qi->
10225 data[cartesian_trajectory_planner_B.ix]);
10226 }
10227
10228 cartesian_trajectory_planner_B.ix = qi->size[0] * qi->size[1];
10229 qi->size[0] = lb->size[0];
10230 qi->size[1] = 1;
10231 cartes_emxEnsureCapacity_real_T(qi,
10232 cartesian_trajectory_planner_B.ix);
10233 cartesian_trajectory_planner_B.nx = lb->size[0] - 1;
10234 for (cartesian_trajectory_planner_B.ix = 0;
10235 cartesian_trajectory_planner_B.ix <=
10236 cartesian_trajectory_planner_B.nx;
10237 cartesian_trajectory_planner_B.ix++) {
10238 qi->data[cartesian_trajectory_planner_B.ix] = lb->
10239 data[cartesian_trajectory_planner_B.ix] + e->
10240 data[cartesian_trajectory_planner_B.ix];
10241 }
10242 } else {
10243 guard2 = true;
10244 }
10245 } else {
10246 guard2 = true;
10247 }
10248 }
10249
10250 if (guard2) {
10251 cartesian_trajectory_planner_B.ix = x_tmp_0->size[0];
10252 x_tmp_0->size[0] = x_tmp->size[0];
10253 car_emxEnsureCapacity_boolean_T(x_tmp_0,
10254 cartesian_trajectory_planner_B.ix);
10255 cartesian_trajectory_planner_B.nx = x_tmp->size[0];
10256 for (cartesian_trajectory_planner_B.ix = 0;
10257 cartesian_trajectory_planner_B.ix <
10258 cartesian_trajectory_planner_B.nx;
10259 cartesian_trajectory_planner_B.ix++) {
10260 x_tmp_0->data[cartesian_trajectory_planner_B.ix] = !x_tmp->
10261 data[cartesian_trajectory_planner_B.ix];
10262 }
10263
10264 if (cartesian_trajectory_planne_any(x_tmp_0)) {
10265 cartesian_trajectory_p_isfinite(ub, x);
10266 cartesian_trajectory_planner_B.y_eb = true;
10267 cartesian_trajectory_planner_B.ix = 0;
10268 exitg2 = false;
10269 while ((!exitg2) && (cartesian_trajectory_planner_B.ix + 1 <=
10270 x->size[0])) {
10271 if (!x->data[cartesian_trajectory_planner_B.ix]) {
10272 cartesian_trajectory_planner_B.y_eb = false;
10273 exitg2 = true;
10274 } else {
10275 cartesian_trajectory_planner_B.ix++;
10276 }
10277 }
10278
10279 if (cartesian_trajectory_planner_B.y_eb) {
10280 cartesian_trajectory_planner_B.ub[0] = ub->size[0];
10281 cartesian_trajectory_planner_B.ub[1] = 1.0;
10282 cartesian_trajectory_plan_randn
10283 (cartesian_trajectory_planner_B.ub, qi);
10284 cartesian_trajectory_planner_B.nx = qi->size[0] - 1;
10285 cartesian_trajectory_planner_B.ix = e->size[0] * e->size[1];
10286 e->size[0] = qi->size[0];
10287 e->size[1] = 1;
10288 cartes_emxEnsureCapacity_real_T(e,
10289 cartesian_trajectory_planner_B.ix);
10290 for (cartesian_trajectory_planner_B.ix = 0;
10291 cartesian_trajectory_planner_B.ix <=
10292 cartesian_trajectory_planner_B.nx;
10293 cartesian_trajectory_planner_B.ix++) {
10294 e->data[cartesian_trajectory_planner_B.ix] = fabs(qi->
10295 data[cartesian_trajectory_planner_B.ix]);
10296 }
10297
10298 cartesian_trajectory_planner_B.ix = qi->size[0] * qi->size[1];
10299 qi->size[0] = ub->size[0];
10300 qi->size[1] = 1;
10301 cartes_emxEnsureCapacity_real_T(qi,
10302 cartesian_trajectory_planner_B.ix);
10303 cartesian_trajectory_planner_B.nx = ub->size[0] - 1;
10304 for (cartesian_trajectory_planner_B.ix = 0;
10305 cartesian_trajectory_planner_B.ix <=
10306 cartesian_trajectory_planner_B.nx;
10307 cartesian_trajectory_planner_B.ix++) {
10308 qi->data[cartesian_trajectory_planner_B.ix] = ub->
10309 data[cartesian_trajectory_planner_B.ix] - e->
10310 data[cartesian_trajectory_planner_B.ix];
10311 }
10312 } else {
10313 guard1 = true;
10314 }
10315 } else {
10316 guard1 = true;
10317 }
10318 }
10319
10320 if (guard1) {
10321 cartesian_trajectory_planner_B.ub[0] = ub->size[0];
10322 cartesian_trajectory_planner_B.ub[1] = 1.0;
10323 cartesian_trajectory_plan_randn(cartesian_trajectory_planner_B.ub,
10324 qi);
10325 }
10326 }
10327
10328 if (cartesian_trajectory_planner_B.err >
10329 cartesian_trajectory_planner_B.iter) {
10330 cartesian_trajectory_planner_B.nx = 0;
10331 cartesian_trajectory_planner_B.ix = 0;
10332 } else {
10333 cartesian_trajectory_planner_B.nx = static_cast<int32_T>
10334 (cartesian_trajectory_planner_B.err) - 1;
10335 cartesian_trajectory_planner_B.ix = static_cast<int32_T>
10336 (cartesian_trajectory_planner_B.iter);
10337 }
10338
10339 cartesian_trajectory_planner_B.unnamed_idx_1 =
10340 cartesian_trajectory_planner_B.ix -
10341 cartesian_trajectory_planner_B.nx;
10342 for (cartesian_trajectory_planner_B.ix = 0;
10343 cartesian_trajectory_planner_B.ix <
10344 cartesian_trajectory_planner_B.unnamed_idx_1;
10345 cartesian_trajectory_planner_B.ix++) {
10346 newseed->data[cartesian_trajectory_planner_B.nx +
10347 cartesian_trajectory_planner_B.ix] = qi->
10348 data[cartesian_trajectory_planner_B.ix];
10349 }
10350 }
10351 }
10352
10353 for (cartesian_trajectory_planner_B.ix = 0;
10354 cartesian_trajectory_planner_B.ix < 6;
10355 cartesian_trajectory_planner_B.ix++) {
10356 obj->SeedInternal[cartesian_trajectory_planner_B.ix] = newseed->
10357 data[cartesian_trajectory_planner_B.ix];
10358 }
10359
10360 DampedBFGSwGradientProjection_s(obj, cartesian_trajectory_planner_B.c_xSol,
10361 &cartesian_trajectory_planner_B.exitFlag,
10362 &cartesian_trajectory_planner_B.err,
10363 &cartesian_trajectory_planner_B.iter);
10364 if (cartesian_trajectory_planner_B.err < *solutionInfo_Error) {
10365 for (cartesian_trajectory_planner_B.i_h = 0;
10366 cartesian_trajectory_planner_B.i_h < 6;
10367 cartesian_trajectory_planner_B.i_h++) {
10368 xSol[cartesian_trajectory_planner_B.i_h] =
10369 cartesian_trajectory_planner_B.c_xSol[cartesian_trajectory_planner_B.i_h];
10370 }
10371
10372 *solutionInfo_Error = cartesian_trajectory_planner_B.err;
10373 cartesian_trajectory_planner_B.exitFlagPrev =
10374 cartesian_trajectory_planner_B.exitFlag;
10375 }
10376
10377 (*solutionInfo_RRAttempts)++;
10378 *solutionInfo_Iterations += cartesian_trajectory_planner_B.iter;
10379 }
10380 }
10381
10382 cartesian_tra_emxFree_boolean_T(&x_0);
10383 cartesian_tra_emxFree_boolean_T(&x_tmp_0);
10384 cartesian_tra_emxFree_boolean_T(&x_tmp);
10385 cartesian_tra_emxFree_boolean_T(&x);
10386 cartesian_trajec_emxFree_real_T(&e);
10387 cartesian_trajec_emxFree_real_T(&rn);
10388 cartesian_trajec_emxFree_real_T(&lb);
10389 cartesian_trajec_emxFree_real_T(&ub);
10390 cartesian_trajec_emxFree_real_T(&qi);
10391 cartesian_trajec_emxFree_real_T(&newseed);
10392 *solutionInfo_ExitFlag = cartesian_trajectory_planner_B.exitFlagPrev;
10393 if (*solutionInfo_Error < cartesian_trajectory_planner_B.tol) {
10394 solutionInfo_Status_size[0] = 1;
10395 solutionInfo_Status_size[1] = 7;
10396 for (cartesian_trajectory_planner_B.ix = 0;
10397 cartesian_trajectory_planner_B.ix < 7;
10398 cartesian_trajectory_planner_B.ix++) {
10399 solutionInfo_Status_data[cartesian_trajectory_planner_B.ix] =
10400 tmp_0[cartesian_trajectory_planner_B.ix];
10401 }
10402 } else {
10403 solutionInfo_Status_size[0] = 1;
10404 solutionInfo_Status_size[1] = 14;
10405 for (cartesian_trajectory_planner_B.ix = 0;
10406 cartesian_trajectory_planner_B.ix < 14;
10407 cartesian_trajectory_planner_B.ix++) {
10408 solutionInfo_Status_data[cartesian_trajectory_planner_B.ix] =
10409 tmp[cartesian_trajectory_planner_B.ix];
10410 }
10411 }
10412}
10413
10414static void cart_inverseKinematics_stepImpl(b_inverseKinematics_cartesian_T *obj,
10415 const real_T tform[16], const real_T weights[6], const real_T initialGuess[6],
10416 real_T QSol[6])
10417{
10418 emxArray_real_T_cartesian_tra_T *bodyIndices;
10419 emxArray_real_T_cartesian_tra_T *positionIndices;
10420 x_robotics_manip_internal_Rig_T *obj_0;
10421 emxArray_char_T_cartesian_tra_T *endEffectorName;
10422 w_robotics_manip_internal_Rig_T *body;
10423 emxArray_real_T_cartesian_tra_T *positionMap;
10424 emxArray_real_T_cartesian_tra_T *e;
10425 emxArray_int32_T_cartesian_tr_T *h;
10426 emxArray_real_T_cartesian_tra_T *y;
10427 emxArray_char_T_cartesian_tra_T *bname;
10428 emxArray_real_T_cartesian_tra_T *bodyIndices_0;
10429 boolean_T exitg1;
10430 c_inverseKinematics_setPoseGoal(obj, tform, weights);
10431 for (cartesian_trajectory_planner_B.i = 0; cartesian_trajectory_planner_B.i <
10432 6; cartesian_trajectory_planner_B.i++) {
10433 QSol[cartesian_trajectory_planner_B.i] =
10434 initialGuess[cartesian_trajectory_planner_B.i];
10435 }
10436
10437 cartesian_trajec_emxInit_char_T(&endEffectorName, 2);
10438 RigidBodyTree_validateConfigu_a(obj->RigidBodyTreeInternal, QSol);
10439 cartes_NLPSolverInterface_solve(obj->Solver, QSol,
10440 cartesian_trajectory_planner_B.qvSolRaw, &cartesian_trajectory_planner_B.bid,
10441 &cartesian_trajectory_planner_B.numPositions,
10442 &cartesian_trajectory_planner_B.ndbl, &cartesian_trajectory_planner_B.apnd,
10443 cartesian_trajectory_planner_B.sol_Status_data,
10444 cartesian_trajectory_planner_B.sol_Status_size);
10445 obj_0 = obj->RigidBodyTreeInternal;
10446 cartesian_trajectory_planner_B.partialTrueCount = endEffectorName->size[0] *
10447 endEffectorName->size[1];
10448 endEffectorName->size[0] = 1;
10449 endEffectorName->size[1] = obj->Solver->ExtraArgs->BodyName->size[1];
10450 cartes_emxEnsureCapacity_char_T(endEffectorName,
10451 cartesian_trajectory_planner_B.partialTrueCount);
10452 cartesian_trajectory_planner_B.loop_ub = obj->Solver->ExtraArgs->
10453 BodyName->size[0] * obj->Solver->ExtraArgs->BodyName->size[1] - 1;
10454 for (cartesian_trajectory_planner_B.partialTrueCount = 0;
10455 cartesian_trajectory_planner_B.partialTrueCount <=
10456 cartesian_trajectory_planner_B.loop_ub;
10457 cartesian_trajectory_planner_B.partialTrueCount++) {
10458 endEffectorName->data[cartesian_trajectory_planner_B.partialTrueCount] =
10459 obj->Solver->ExtraArgs->BodyName->
10460 data[cartesian_trajectory_planner_B.partialTrueCount];
10461 }
10462
10463 cartesian_trajec_emxInit_real_T(&bodyIndices, 1);
10464 cartesian_trajectory_planner_B.partialTrueCount = bodyIndices->size[0];
10465 bodyIndices->size[0] = static_cast<int32_T>(obj_0->NumBodies);
10466 cartes_emxEnsureCapacity_real_T(bodyIndices,
10467 cartesian_trajectory_planner_B.partialTrueCount);
10468 cartesian_trajectory_planner_B.loop_ub = static_cast<int32_T>(obj_0->NumBodies);
10469 for (cartesian_trajectory_planner_B.partialTrueCount = 0;
10470 cartesian_trajectory_planner_B.partialTrueCount <
10471 cartesian_trajectory_planner_B.loop_ub;
10472 cartesian_trajectory_planner_B.partialTrueCount++) {
10473 bodyIndices->data[cartesian_trajectory_planner_B.partialTrueCount] = 0.0;
10474 }
10475
10476 cartesian_trajec_emxInit_char_T(&bname, 2);
10477 cartesian_trajectory_planner_B.bid = -1.0;
10478 cartesian_trajectory_planner_B.partialTrueCount = bname->size[0] * bname->
10479 size[1];
10480 bname->size[0] = 1;
10481 bname->size[1] = obj_0->Base.NameInternal->size[1];
10482 cartes_emxEnsureCapacity_char_T(bname,
10483 cartesian_trajectory_planner_B.partialTrueCount);
10484 cartesian_trajectory_planner_B.loop_ub = obj_0->Base.NameInternal->size[0] *
10485 obj_0->Base.NameInternal->size[1] - 1;
10486 for (cartesian_trajectory_planner_B.partialTrueCount = 0;
10487 cartesian_trajectory_planner_B.partialTrueCount <=
10488 cartesian_trajectory_planner_B.loop_ub;
10489 cartesian_trajectory_planner_B.partialTrueCount++) {
10490 bname->data[cartesian_trajectory_planner_B.partialTrueCount] =
10491 obj_0->Base.NameInternal->
10492 data[cartesian_trajectory_planner_B.partialTrueCount];
10493 }
10494
10495 if (cartesian_trajectory_pla_strcmp(bname, endEffectorName)) {
10496 cartesian_trajectory_planner_B.bid = 0.0;
10497 } else {
10498 cartesian_trajectory_planner_B.numPositions = obj_0->NumBodies;
10499 cartesian_trajectory_planner_B.i = 0;
10500 exitg1 = false;
10501 while ((!exitg1) && (cartesian_trajectory_planner_B.i <= static_cast<int32_T>
10502 (cartesian_trajectory_planner_B.numPositions) - 1)) {
10503 body = obj_0->Bodies[cartesian_trajectory_planner_B.i];
10504 cartesian_trajectory_planner_B.partialTrueCount = bname->size[0] *
10505 bname->size[1];
10506 bname->size[0] = 1;
10507 bname->size[1] = body->NameInternal->size[1];
10508 cartes_emxEnsureCapacity_char_T(bname,
10509 cartesian_trajectory_planner_B.partialTrueCount);
10510 cartesian_trajectory_planner_B.loop_ub = body->NameInternal->size[0] *
10511 body->NameInternal->size[1] - 1;
10512 for (cartesian_trajectory_planner_B.partialTrueCount = 0;
10513 cartesian_trajectory_planner_B.partialTrueCount <=
10514 cartesian_trajectory_planner_B.loop_ub;
10515 cartesian_trajectory_planner_B.partialTrueCount++) {
10516 bname->data[cartesian_trajectory_planner_B.partialTrueCount] =
10517 body->NameInternal->
10518 data[cartesian_trajectory_planner_B.partialTrueCount];
10519 }
10520
10521 if (cartesian_trajectory_pla_strcmp(bname, endEffectorName)) {
10522 cartesian_trajectory_planner_B.bid = static_cast<real_T>
10523 (cartesian_trajectory_planner_B.i) + 1.0;
10524 exitg1 = true;
10525 } else {
10526 cartesian_trajectory_planner_B.i++;
10527 }
10528 }
10529 }
10530
10531 cartesian_trajec_emxFree_char_T(&bname);
10532 cartesian_trajec_emxFree_char_T(&endEffectorName);
10533 if (cartesian_trajectory_planner_B.bid == 0.0) {
10534 cartesian_trajectory_planner_B.partialTrueCount = bodyIndices->size[0];
10535 bodyIndices->size[0] = 1;
10536 cartes_emxEnsureCapacity_real_T(bodyIndices,
10537 cartesian_trajectory_planner_B.partialTrueCount);
10538 bodyIndices->data[0] = 0.0;
10539 } else {
10540 body = obj_0->Bodies[static_cast<int32_T>(cartesian_trajectory_planner_B.bid)
10541 - 1];
10542 cartesian_trajectory_planner_B.bid = 1.0;
10543 while (body->ParentIndex != 0.0) {
10544 bodyIndices->data[static_cast<int32_T>(cartesian_trajectory_planner_B.bid)
10545 - 1] = body->Index;
10546 body = obj_0->Bodies[static_cast<int32_T>(body->ParentIndex) - 1];
10547 cartesian_trajectory_planner_B.bid++;
10548 }
10549
10550 if (1.0 > cartesian_trajectory_planner_B.bid - 1.0) {
10551 cartesian_trajectory_planner_B.c_k = -1;
10552 } else {
10553 cartesian_trajectory_planner_B.c_k = static_cast<int32_T>
10554 (cartesian_trajectory_planner_B.bid - 1.0) - 1;
10555 }
10556
10557 cartesian_trajec_emxInit_real_T(&bodyIndices_0, 1);
10558 cartesian_trajectory_planner_B.partialTrueCount = bodyIndices_0->size[0];
10559 bodyIndices_0->size[0] = cartesian_trajectory_planner_B.c_k + 3;
10560 cartes_emxEnsureCapacity_real_T(bodyIndices_0,
10561 cartesian_trajectory_planner_B.partialTrueCount);
10562 for (cartesian_trajectory_planner_B.partialTrueCount = 0;
10563 cartesian_trajectory_planner_B.partialTrueCount <=
10564 cartesian_trajectory_planner_B.c_k;
10565 cartesian_trajectory_planner_B.partialTrueCount++) {
10566 bodyIndices_0->data[cartesian_trajectory_planner_B.partialTrueCount] =
10567 bodyIndices->data[cartesian_trajectory_planner_B.partialTrueCount];
10568 }
10569
10570 bodyIndices_0->data[cartesian_trajectory_planner_B.c_k + 1] = body->Index;
10571 bodyIndices_0->data[cartesian_trajectory_planner_B.c_k + 2] = 0.0;
10572 cartesian_trajectory_planner_B.partialTrueCount = bodyIndices->size[0];
10573 bodyIndices->size[0] = bodyIndices_0->size[0];
10574 cartes_emxEnsureCapacity_real_T(bodyIndices,
10575 cartesian_trajectory_planner_B.partialTrueCount);
10576 cartesian_trajectory_planner_B.loop_ub = bodyIndices_0->size[0];
10577 for (cartesian_trajectory_planner_B.partialTrueCount = 0;
10578 cartesian_trajectory_planner_B.partialTrueCount <
10579 cartesian_trajectory_planner_B.loop_ub;
10580 cartesian_trajectory_planner_B.partialTrueCount++) {
10581 bodyIndices->data[cartesian_trajectory_planner_B.partialTrueCount] =
10582 bodyIndices_0->data[cartesian_trajectory_planner_B.partialTrueCount];
10583 }
10584
10585 cartesian_trajec_emxFree_real_T(&bodyIndices_0);
10586 }
10587
10588 obj_0 = obj->RigidBodyTreeInternal;
10589 cartesian_trajectory_planner_B.c_k = bodyIndices->size[0] - 1;
10590 cartesian_trajectory_planner_B.loop_ub = 0;
10591 for (cartesian_trajectory_planner_B.i = 0; cartesian_trajectory_planner_B.i <=
10592 cartesian_trajectory_planner_B.c_k; cartesian_trajectory_planner_B.i++) {
10593 if (bodyIndices->data[cartesian_trajectory_planner_B.i] != 0.0) {
10594 cartesian_trajectory_planner_B.loop_ub++;
10595 }
10596 }
10597
10598 cartesian_traje_emxInit_int32_T(&h, 1);
10599 cartesian_trajectory_planner_B.partialTrueCount = h->size[0];
10600 h->size[0] = cartesian_trajectory_planner_B.loop_ub;
10601 carte_emxEnsureCapacity_int32_T(h,
10602 cartesian_trajectory_planner_B.partialTrueCount);
10603 cartesian_trajectory_planner_B.partialTrueCount = 0;
10604 for (cartesian_trajectory_planner_B.i = 0; cartesian_trajectory_planner_B.i <=
10605 cartesian_trajectory_planner_B.c_k; cartesian_trajectory_planner_B.i++) {
10606 if (bodyIndices->data[cartesian_trajectory_planner_B.i] != 0.0) {
10607 h->data[cartesian_trajectory_planner_B.partialTrueCount] =
10608 cartesian_trajectory_planner_B.i + 1;
10609 cartesian_trajectory_planner_B.partialTrueCount++;
10610 }
10611 }
10612
10613 cartesian_trajec_emxInit_real_T(&positionMap, 2);
10614 cartesian_trajectory_planner_B.partialTrueCount = positionMap->size[0] *
10615 positionMap->size[1];
10616 positionMap->size[0] = h->size[0];
10617 positionMap->size[1] = 2;
10618 cartes_emxEnsureCapacity_real_T(positionMap,
10619 cartesian_trajectory_planner_B.partialTrueCount);
10620 cartesian_trajectory_planner_B.loop_ub = h->size[0];
10621 for (cartesian_trajectory_planner_B.partialTrueCount = 0;
10622 cartesian_trajectory_planner_B.partialTrueCount <
10623 cartesian_trajectory_planner_B.loop_ub;
10624 cartesian_trajectory_planner_B.partialTrueCount++) {
10625 positionMap->data[cartesian_trajectory_planner_B.partialTrueCount] =
10626 obj_0->PositionDoFMap[static_cast<int32_T>(bodyIndices->data[h->
10627 data[cartesian_trajectory_planner_B.partialTrueCount] - 1]) - 1];
10628 }
10629
10630 cartesian_trajectory_planner_B.loop_ub = h->size[0];
10631 for (cartesian_trajectory_planner_B.partialTrueCount = 0;
10632 cartesian_trajectory_planner_B.partialTrueCount <
10633 cartesian_trajectory_planner_B.loop_ub;
10634 cartesian_trajectory_planner_B.partialTrueCount++) {
10635 positionMap->data[cartesian_trajectory_planner_B.partialTrueCount +
10636 positionMap->size[0]] = obj_0->PositionDoFMap[static_cast<int32_T>
10637 (bodyIndices->data[h->data[cartesian_trajectory_planner_B.partialTrueCount]
10638 - 1]) + 7];
10639 }
10640
10641 cartesian_traje_emxFree_int32_T(&h);
10642 cartesian_trajec_emxFree_real_T(&bodyIndices);
10643 cartesian_trajec_emxInit_real_T(&positionIndices, 2);
10644 cartesian_trajectory_planner_B.partialTrueCount = positionIndices->size[0] *
10645 positionIndices->size[1];
10646 positionIndices->size[0] = 1;
10647 positionIndices->size[1] = static_cast<int32_T>(obj_0->PositionNumber);
10648 cartes_emxEnsureCapacity_real_T(positionIndices,
10649 cartesian_trajectory_planner_B.partialTrueCount);
10650 cartesian_trajectory_planner_B.loop_ub = static_cast<int32_T>
10651 (obj_0->PositionNumber) - 1;
10652 for (cartesian_trajectory_planner_B.partialTrueCount = 0;
10653 cartesian_trajectory_planner_B.partialTrueCount <=
10654 cartesian_trajectory_planner_B.loop_ub;
10655 cartesian_trajectory_planner_B.partialTrueCount++) {
10656 positionIndices->data[cartesian_trajectory_planner_B.partialTrueCount] = 0.0;
10657 }
10658
10659 cartesian_trajectory_planner_B.bid = 0.0;
10660 cartesian_trajectory_planner_B.c_k = positionMap->size[0] - 1;
10661 cartesian_trajec_emxInit_real_T(&e, 2);
10662 cartesian_trajec_emxInit_real_T(&y, 2);
10663 for (cartesian_trajectory_planner_B.i = 0; cartesian_trajectory_planner_B.i <=
10664 cartesian_trajectory_planner_B.c_k; cartesian_trajectory_planner_B.i++) {
10665 cartesian_trajectory_planner_B.numPositions = (positionMap->
10666 data[cartesian_trajectory_planner_B.i + positionMap->size[0]] -
10667 positionMap->data[cartesian_trajectory_planner_B.i]) + 1.0;
10668 if (cartesian_trajectory_planner_B.numPositions > 0.0) {
10669 if (cartesian_trajectory_planner_B.numPositions < 1.0) {
10670 y->size[0] = 1;
10671 y->size[1] = 0;
10672 } else if (rtIsInf(cartesian_trajectory_planner_B.numPositions) && (1.0 ==
10673 cartesian_trajectory_planner_B.numPositions)) {
10674 cartesian_trajectory_planner_B.partialTrueCount = y->size[0] * y->size[1];
10675 y->size[0] = 1;
10676 y->size[1] = 1;
10677 cartes_emxEnsureCapacity_real_T(y,
10678 cartesian_trajectory_planner_B.partialTrueCount);
10679 y->data[0] = (rtNaN);
10680 } else {
10681 cartesian_trajectory_planner_B.partialTrueCount = y->size[0] * y->size[1];
10682 y->size[0] = 1;
10683 cartesian_trajectory_planner_B.loop_ub = static_cast<int32_T>(floor
10684 (cartesian_trajectory_planner_B.numPositions - 1.0));
10685 y->size[1] = cartesian_trajectory_planner_B.loop_ub + 1;
10686 cartes_emxEnsureCapacity_real_T(y,
10687 cartesian_trajectory_planner_B.partialTrueCount);
10688 for (cartesian_trajectory_planner_B.partialTrueCount = 0;
10689 cartesian_trajectory_planner_B.partialTrueCount <=
10690 cartesian_trajectory_planner_B.loop_ub;
10691 cartesian_trajectory_planner_B.partialTrueCount++) {
10692 y->data[cartesian_trajectory_planner_B.partialTrueCount] =
10693 static_cast<real_T>(cartesian_trajectory_planner_B.partialTrueCount)
10694 + 1.0;
10695 }
10696 }
10697
10698 if (rtIsNaN(positionMap->data[cartesian_trajectory_planner_B.i]) ||
10699 rtIsNaN(positionMap->data[cartesian_trajectory_planner_B.i +
10700 positionMap->size[0]])) {
10701 cartesian_trajectory_planner_B.partialTrueCount = e->size[0] * e->size[1];
10702 e->size[0] = 1;
10703 e->size[1] = 1;
10704 cartes_emxEnsureCapacity_real_T(e,
10705 cartesian_trajectory_planner_B.partialTrueCount);
10706 e->data[0] = (rtNaN);
10707 } else if (positionMap->data[cartesian_trajectory_planner_B.i +
10708 positionMap->size[0]] < positionMap->
10709 data[cartesian_trajectory_planner_B.i]) {
10710 e->size[0] = 1;
10711 e->size[1] = 0;
10712 } else if ((rtIsInf(positionMap->data[cartesian_trajectory_planner_B.i]) ||
10713 rtIsInf(positionMap->data[cartesian_trajectory_planner_B.i +
10714 positionMap->size[0]])) && (positionMap->
10715 data[cartesian_trajectory_planner_B.i + positionMap->size[0]] ==
10716 positionMap->data[cartesian_trajectory_planner_B.i])) {
10717 cartesian_trajectory_planner_B.partialTrueCount = e->size[0] * e->size[1];
10718 e->size[0] = 1;
10719 e->size[1] = 1;
10720 cartes_emxEnsureCapacity_real_T(e,
10721 cartesian_trajectory_planner_B.partialTrueCount);
10722 e->data[0] = (rtNaN);
10723 } else if (floor(positionMap->data[cartesian_trajectory_planner_B.i]) ==
10724 positionMap->data[cartesian_trajectory_planner_B.i]) {
10725 cartesian_trajectory_planner_B.partialTrueCount = e->size[0] * e->size[1];
10726 e->size[0] = 1;
10727 e->size[1] = static_cast<int32_T>(floor(positionMap->
10728 data[cartesian_trajectory_planner_B.i + positionMap->size[0]] -
10729 positionMap->data[cartesian_trajectory_planner_B.i])) + 1;
10730 cartes_emxEnsureCapacity_real_T(e,
10731 cartesian_trajectory_planner_B.partialTrueCount);
10732 cartesian_trajectory_planner_B.loop_ub = static_cast<int32_T>(floor
10733 (positionMap->data[cartesian_trajectory_planner_B.i +
10734 positionMap->size[0]] - positionMap->
10735 data[cartesian_trajectory_planner_B.i]));
10736 for (cartesian_trajectory_planner_B.partialTrueCount = 0;
10737 cartesian_trajectory_planner_B.partialTrueCount <=
10738 cartesian_trajectory_planner_B.loop_ub;
10739 cartesian_trajectory_planner_B.partialTrueCount++) {
10740 e->data[cartesian_trajectory_planner_B.partialTrueCount] =
10741 positionMap->data[cartesian_trajectory_planner_B.i] +
10742 static_cast<real_T>(cartesian_trajectory_planner_B.partialTrueCount);
10743 }
10744 } else {
10745 cartesian_trajectory_planner_B.ndbl = floor((positionMap->
10746 data[cartesian_trajectory_planner_B.i + positionMap->size[0]] -
10747 positionMap->data[cartesian_trajectory_planner_B.i]) + 0.5);
10748 cartesian_trajectory_planner_B.apnd = positionMap->
10749 data[cartesian_trajectory_planner_B.i] +
10750 cartesian_trajectory_planner_B.ndbl;
10751 cartesian_trajectory_planner_B.cdiff =
10752 cartesian_trajectory_planner_B.apnd - positionMap->
10753 data[cartesian_trajectory_planner_B.i + positionMap->size[0]];
10754 cartesian_trajectory_planner_B.u0 = fabs(positionMap->
10755 data[cartesian_trajectory_planner_B.i]);
10756 cartesian_trajectory_planner_B.u1 = fabs(positionMap->
10757 data[cartesian_trajectory_planner_B.i + positionMap->size[0]]);
10758 if ((cartesian_trajectory_planner_B.u0 >
10759 cartesian_trajectory_planner_B.u1) || rtIsNaN
10760 (cartesian_trajectory_planner_B.u1)) {
10761 cartesian_trajectory_planner_B.u1 = cartesian_trajectory_planner_B.u0;
10762 }
10763
10764 if (fabs(cartesian_trajectory_planner_B.cdiff) < 4.4408920985006262E-16 *
10765 cartesian_trajectory_planner_B.u1) {
10766 cartesian_trajectory_planner_B.ndbl++;
10767 cartesian_trajectory_planner_B.apnd = positionMap->
10768 data[cartesian_trajectory_planner_B.i + positionMap->size[0]];
10769 } else if (cartesian_trajectory_planner_B.cdiff > 0.0) {
10770 cartesian_trajectory_planner_B.apnd =
10771 (cartesian_trajectory_planner_B.ndbl - 1.0) + positionMap->
10772 data[cartesian_trajectory_planner_B.i];
10773 } else {
10774 cartesian_trajectory_planner_B.ndbl++;
10775 }
10776
10777 if (cartesian_trajectory_planner_B.ndbl >= 0.0) {
10778 cartesian_trajectory_planner_B.partialTrueCount = static_cast<int32_T>
10779 (cartesian_trajectory_planner_B.ndbl);
10780 } else {
10781 cartesian_trajectory_planner_B.partialTrueCount = 0;
10782 }
10783
10784 cartesian_trajectory_planner_B.loop_ub =
10785 cartesian_trajectory_planner_B.partialTrueCount - 1;
10786 cartesian_trajectory_planner_B.partialTrueCount = e->size[0] * e->size[1];
10787 e->size[0] = 1;
10788 e->size[1] = cartesian_trajectory_planner_B.loop_ub + 1;
10789 cartes_emxEnsureCapacity_real_T(e,
10790 cartesian_trajectory_planner_B.partialTrueCount);
10791 if (cartesian_trajectory_planner_B.loop_ub + 1 > 0) {
10792 e->data[0] = positionMap->data[cartesian_trajectory_planner_B.i];
10793 if (cartesian_trajectory_planner_B.loop_ub + 1 > 1) {
10794 e->data[cartesian_trajectory_planner_B.loop_ub] =
10795 cartesian_trajectory_planner_B.apnd;
10796 cartesian_trajectory_planner_B.nm1d2 =
10797 ((cartesian_trajectory_planner_B.loop_ub < 0) +
10798 cartesian_trajectory_planner_B.loop_ub) >> 1;
10799 cartesian_trajectory_planner_B.c_o =
10800 cartesian_trajectory_planner_B.nm1d2 - 2;
10801 for (cartesian_trajectory_planner_B.partialTrueCount = 0;
10802 cartesian_trajectory_planner_B.partialTrueCount <=
10803 cartesian_trajectory_planner_B.c_o;
10804 cartesian_trajectory_planner_B.partialTrueCount++) {
10805 cartesian_trajectory_planner_B.k_j =
10806 cartesian_trajectory_planner_B.partialTrueCount + 1;
10807 e->data[cartesian_trajectory_planner_B.k_j] = positionMap->
10808 data[cartesian_trajectory_planner_B.i] + static_cast<real_T>
10809 (cartesian_trajectory_planner_B.k_j);
10810 e->data[cartesian_trajectory_planner_B.loop_ub -
10811 cartesian_trajectory_planner_B.k_j] =
10812 cartesian_trajectory_planner_B.apnd - static_cast<real_T>
10813 (cartesian_trajectory_planner_B.k_j);
10814 }
10815
10816 if (cartesian_trajectory_planner_B.nm1d2 << 1 ==
10817 cartesian_trajectory_planner_B.loop_ub) {
10818 e->data[cartesian_trajectory_planner_B.nm1d2] = (positionMap->
10819 data[cartesian_trajectory_planner_B.i] +
10820 cartesian_trajectory_planner_B.apnd) / 2.0;
10821 } else {
10822 e->data[cartesian_trajectory_planner_B.nm1d2] = positionMap->
10823 data[cartesian_trajectory_planner_B.i] + static_cast<real_T>
10824 (cartesian_trajectory_planner_B.nm1d2);
10825 e->data[cartesian_trajectory_planner_B.nm1d2 + 1] =
10826 cartesian_trajectory_planner_B.apnd - static_cast<real_T>
10827 (cartesian_trajectory_planner_B.nm1d2);
10828 }
10829 }
10830 }
10831 }
10832
10833 cartesian_trajectory_planner_B.partialTrueCount = e->size[0] * e->size[1];
10834 cartesian_trajectory_planner_B.loop_ub =
10835 cartesian_trajectory_planner_B.partialTrueCount - 1;
10836 for (cartesian_trajectory_planner_B.partialTrueCount = 0;
10837 cartesian_trajectory_planner_B.partialTrueCount <=
10838 cartesian_trajectory_planner_B.loop_ub;
10839 cartesian_trajectory_planner_B.partialTrueCount++) {
10840 positionIndices->data[static_cast<int32_T>
10841 (cartesian_trajectory_planner_B.bid + y->
10842 data[cartesian_trajectory_planner_B.partialTrueCount]) - 1] = e->
10843 data[cartesian_trajectory_planner_B.partialTrueCount];
10844 }
10845
10846 cartesian_trajectory_planner_B.bid +=
10847 cartesian_trajectory_planner_B.numPositions;
10848 }
10849 }
10850
10851 cartesian_trajec_emxFree_real_T(&y);
10852 cartesian_trajec_emxFree_real_T(&e);
10853 cartesian_trajec_emxFree_real_T(&positionMap);
10854 if (1.0 > cartesian_trajectory_planner_B.bid) {
10855 positionIndices->size[1] = 0;
10856 } else {
10857 cartesian_trajectory_planner_B.partialTrueCount = positionIndices->size[0] *
10858 positionIndices->size[1];
10859 positionIndices->size[1] = static_cast<int32_T>
10860 (cartesian_trajectory_planner_B.bid);
10861 cartes_emxEnsureCapacity_real_T(positionIndices,
10862 cartesian_trajectory_planner_B.partialTrueCount);
10863 }
10864
10865 cartesian_trajectory_planner_B.loop_ub = positionIndices->size[0] *
10866 positionIndices->size[1];
10867 for (cartesian_trajectory_planner_B.partialTrueCount = 0;
10868 cartesian_trajectory_planner_B.partialTrueCount <
10869 cartesian_trajectory_planner_B.loop_ub;
10870 cartesian_trajectory_planner_B.partialTrueCount++) {
10871 QSol[static_cast<int32_T>(positionIndices->
10872 data[cartesian_trajectory_planner_B.partialTrueCount]) - 1] =
10873 cartesian_trajectory_planner_B.qvSolRaw[static_cast<int32_T>
10874 (positionIndices->data[cartesian_trajectory_planner_B.partialTrueCount]) -
10875 1];
10876 }
10877
10878 cartesian_trajec_emxFree_real_T(&positionIndices);
10879}
10880
10881static void cartesian_t_emxInit_f_cell_wrap(emxArray_f_cell_wrap_cartesia_T
10882 **pEmxArray, int32_T numDimensions)
10883{
10884 emxArray_f_cell_wrap_cartesia_T *emxArray;
10885 int32_T i;
10886 *pEmxArray = (emxArray_f_cell_wrap_cartesia_T *)malloc(sizeof
10887 (emxArray_f_cell_wrap_cartesia_T));
10888 emxArray = *pEmxArray;
10889 emxArray->data = (f_cell_wrap_cartesian_traject_T *)NULL;
10890 emxArray->numDimensions = numDimensions;
10891 emxArray->size = (int32_T *)malloc(sizeof(int32_T) * numDimensions);
10892 emxArray->allocatedSize = 0;
10893 emxArray->canFreeData = true;
10894 for (i = 0; i < numDimensions; i++) {
10895 emxArray->size[i] = 0;
10896 }
10897}
10898
10899static void c_emxEnsureCapacity_f_cell_wrap(emxArray_f_cell_wrap_cartesia_T
10900 *emxArray, int32_T oldNumel)
10901{
10902 int32_T newNumel;
10903 int32_T i;
10904 void *newData;
10905 if (oldNumel < 0) {
10906 oldNumel = 0;
10907 }
10908
10909 newNumel = 1;
10910 for (i = 0; i < emxArray->numDimensions; i++) {
10911 newNumel *= emxArray->size[i];
10912 }
10913
10914 if (newNumel > emxArray->allocatedSize) {
10915 i = emxArray->allocatedSize;
10916 if (i < 16) {
10917 i = 16;
10918 }
10919
10920 while (i < newNumel) {
10921 if (i > 1073741823) {
10922 i = MAX_int32_T;
10923 } else {
10924 i <<= 1;
10925 }
10926 }
10927
10928 newData = calloc(static_cast<uint32_T>(i), sizeof
10929 (f_cell_wrap_cartesian_traject_T));
10930 if (emxArray->data != NULL) {
10931 memcpy(newData, emxArray->data, sizeof(f_cell_wrap_cartesian_traject_T)
10932 * oldNumel);
10933 if (emxArray->canFreeData) {
10934 free(emxArray->data);
10935 }
10936 }
10937
10938 emxArray->data = (f_cell_wrap_cartesian_traject_T *)newData;
10939 emxArray->allocatedSize = i;
10940 emxArray->canFreeData = true;
10941 }
10942}
10943
10944static void ca_rigidBodyJoint_get_JointAxis(const
10945 c_rigidBodyJoint_cartesian_tr_T *obj, real_T ax[3])
10946{
10947 emxArray_char_T_cartesian_tra_T *a;
10948 static const char_T tmp[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
10949
10950 static const char_T tmp_0[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
10951
10952 boolean_T guard1 = false;
10953 int32_T exitg1;
10954 cartesian_trajec_emxInit_char_T(&a, 2);
10955 cartesian_trajectory_planner_B.b_kstr_j = a->size[0] * a->size[1];
10956 a->size[0] = 1;
10957 a->size[1] = obj->Type->size[1];
10958 cartes_emxEnsureCapacity_char_T(a, cartesian_trajectory_planner_B.b_kstr_j);
10959 cartesian_trajectory_planner_B.loop_ub_o4 = obj->Type->size[0] * obj->
10960 Type->size[1] - 1;
10961 for (cartesian_trajectory_planner_B.b_kstr_j = 0;
10962 cartesian_trajectory_planner_B.b_kstr_j <=
10963 cartesian_trajectory_planner_B.loop_ub_o4;
10964 cartesian_trajectory_planner_B.b_kstr_j++) {
10965 a->data[cartesian_trajectory_planner_B.b_kstr_j] = obj->Type->
10966 data[cartesian_trajectory_planner_B.b_kstr_j];
10967 }
10968
10969 for (cartesian_trajectory_planner_B.b_kstr_j = 0;
10970 cartesian_trajectory_planner_B.b_kstr_j < 8;
10971 cartesian_trajectory_planner_B.b_kstr_j++) {
10972 cartesian_trajectory_planner_B.b_n3[cartesian_trajectory_planner_B.b_kstr_j]
10973 = tmp[cartesian_trajectory_planner_B.b_kstr_j];
10974 }
10975
10976 cartesian_trajectory_planner_B.b_bool_nh = false;
10977 if (a->size[1] == 8) {
10978 cartesian_trajectory_planner_B.b_kstr_j = 1;
10979 do {
10980 exitg1 = 0;
10981 if (cartesian_trajectory_planner_B.b_kstr_j - 1 < 8) {
10982 cartesian_trajectory_planner_B.loop_ub_o4 =
10983 cartesian_trajectory_planner_B.b_kstr_j - 1;
10984 if (a->data[cartesian_trajectory_planner_B.loop_ub_o4] !=
10985 cartesian_trajectory_planner_B.b_n3[cartesian_trajectory_planner_B.loop_ub_o4])
10986 {
10987 exitg1 = 1;
10988 } else {
10989 cartesian_trajectory_planner_B.b_kstr_j++;
10990 }
10991 } else {
10992 cartesian_trajectory_planner_B.b_bool_nh = true;
10993 exitg1 = 1;
10994 }
10995 } while (exitg1 == 0);
10996 }
10997
10998 guard1 = false;
10999 if (cartesian_trajectory_planner_B.b_bool_nh) {
11000 guard1 = true;
11001 } else {
11002 cartesian_trajectory_planner_B.b_kstr_j = a->size[0] * a->size[1];
11003 a->size[0] = 1;
11004 a->size[1] = obj->Type->size[1];
11005 cartes_emxEnsureCapacity_char_T(a, cartesian_trajectory_planner_B.b_kstr_j);
11006 cartesian_trajectory_planner_B.loop_ub_o4 = obj->Type->size[0] * obj->
11007 Type->size[1] - 1;
11008 for (cartesian_trajectory_planner_B.b_kstr_j = 0;
11009 cartesian_trajectory_planner_B.b_kstr_j <=
11010 cartesian_trajectory_planner_B.loop_ub_o4;
11011 cartesian_trajectory_planner_B.b_kstr_j++) {
11012 a->data[cartesian_trajectory_planner_B.b_kstr_j] = obj->Type->
11013 data[cartesian_trajectory_planner_B.b_kstr_j];
11014 }
11015
11016 for (cartesian_trajectory_planner_B.b_kstr_j = 0;
11017 cartesian_trajectory_planner_B.b_kstr_j < 9;
11018 cartesian_trajectory_planner_B.b_kstr_j++) {
11019 cartesian_trajectory_planner_B.b_n[cartesian_trajectory_planner_B.b_kstr_j]
11020 = tmp_0[cartesian_trajectory_planner_B.b_kstr_j];
11021 }
11022
11023 cartesian_trajectory_planner_B.b_bool_nh = false;
11024 if (a->size[1] == 9) {
11025 cartesian_trajectory_planner_B.b_kstr_j = 1;
11026 do {
11027 exitg1 = 0;
11028 if (cartesian_trajectory_planner_B.b_kstr_j - 1 < 9) {
11029 cartesian_trajectory_planner_B.loop_ub_o4 =
11030 cartesian_trajectory_planner_B.b_kstr_j - 1;
11031 if (a->data[cartesian_trajectory_planner_B.loop_ub_o4] !=
11032 cartesian_trajectory_planner_B.b_n[cartesian_trajectory_planner_B.loop_ub_o4])
11033 {
11034 exitg1 = 1;
11035 } else {
11036 cartesian_trajectory_planner_B.b_kstr_j++;
11037 }
11038 } else {
11039 cartesian_trajectory_planner_B.b_bool_nh = true;
11040 exitg1 = 1;
11041 }
11042 } while (exitg1 == 0);
11043 }
11044
11045 if (cartesian_trajectory_planner_B.b_bool_nh) {
11046 guard1 = true;
11047 } else {
11048 ax[0] = (rtNaN);
11049 ax[1] = (rtNaN);
11050 ax[2] = (rtNaN);
11051 }
11052 }
11053
11054 if (guard1) {
11055 ax[0] = obj->JointAxisInternal[0];
11056 ax[1] = obj->JointAxisInternal[1];
11057 ax[2] = obj->JointAxisInternal[2];
11058 }
11059
11060 cartesian_trajec_emxFree_char_T(&a);
11061}
11062
11063static void RigidBodyTree_forwardKinematics(p_robotics_manip_internal_Rig_T *obj,
11064 const real_T qvec[6], emxArray_f_cell_wrap_cartesia_T *Ttree)
11065{
11066 n_robotics_manip_internal_Rig_T *body;
11067 emxArray_char_T_cartesian_tra_T *switch_expression;
11068 static const int8_T tmp[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1
11069 };
11070
11071 static const char_T tmp_0[5] = { 'f', 'i', 'x', 'e', 'd' };
11072
11073 static const char_T tmp_1[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
11074
11075 int32_T exitg1;
11076 cartesian_trajectory_planner_B.n = obj->NumBodies;
11077 for (cartesian_trajectory_planner_B.b_kstr_i = 0;
11078 cartesian_trajectory_planner_B.b_kstr_i < 16;
11079 cartesian_trajectory_planner_B.b_kstr_i++) {
11080 cartesian_trajectory_planner_B.c_f1[cartesian_trajectory_planner_B.b_kstr_i]
11081 = tmp[cartesian_trajectory_planner_B.b_kstr_i];
11082 }
11083
11084 cartesian_trajectory_planner_B.b_kstr_i = Ttree->size[0] * Ttree->size[1];
11085 Ttree->size[0] = 1;
11086 cartesian_trajectory_planner_B.e_on = static_cast<int32_T>
11087 (cartesian_trajectory_planner_B.n);
11088 Ttree->size[1] = cartesian_trajectory_planner_B.e_on;
11089 c_emxEnsureCapacity_f_cell_wrap(Ttree, cartesian_trajectory_planner_B.b_kstr_i);
11090 if (cartesian_trajectory_planner_B.e_on != 0) {
11091 cartesian_trajectory_planner_B.ntilecols =
11092 cartesian_trajectory_planner_B.e_on - 1;
11093 if (0 <= cartesian_trajectory_planner_B.ntilecols) {
11094 memcpy(&cartesian_trajectory_planner_B.expl_temp.f1[0],
11095 &cartesian_trajectory_planner_B.c_f1[0], sizeof(real_T) << 4U);
11096 }
11097
11098 for (cartesian_trajectory_planner_B.b_jtilecol = 0;
11099 cartesian_trajectory_planner_B.b_jtilecol <=
11100 cartesian_trajectory_planner_B.ntilecols;
11101 cartesian_trajectory_planner_B.b_jtilecol++) {
11102 Ttree->data[cartesian_trajectory_planner_B.b_jtilecol] =
11103 cartesian_trajectory_planner_B.expl_temp;
11104 }
11105 }
11106
11107 cartesian_trajectory_planner_B.k = 1.0;
11108 cartesian_trajectory_planner_B.ntilecols = static_cast<int32_T>
11109 (cartesian_trajectory_planner_B.n) - 1;
11110 cartesian_trajec_emxInit_char_T(&switch_expression, 2);
11111 if (0 <= cartesian_trajectory_planner_B.ntilecols) {
11112 for (cartesian_trajectory_planner_B.b_kstr_i = 0;
11113 cartesian_trajectory_planner_B.b_kstr_i < 5;
11114 cartesian_trajectory_planner_B.b_kstr_i++) {
11115 cartesian_trajectory_planner_B.b_cf[cartesian_trajectory_planner_B.b_kstr_i]
11116 = tmp_0[cartesian_trajectory_planner_B.b_kstr_i];
11117 }
11118 }
11119
11120 for (cartesian_trajectory_planner_B.b_jtilecol = 0;
11121 cartesian_trajectory_planner_B.b_jtilecol <=
11122 cartesian_trajectory_planner_B.ntilecols;
11123 cartesian_trajectory_planner_B.b_jtilecol++) {
11124 body = obj->Bodies[cartesian_trajectory_planner_B.b_jtilecol];
11125 cartesian_trajectory_planner_B.n = body->JointInternal.PositionNumber;
11126 cartesian_trajectory_planner_B.n += cartesian_trajectory_planner_B.k;
11127 if (cartesian_trajectory_planner_B.k > cartesian_trajectory_planner_B.n -
11128 1.0) {
11129 cartesian_trajectory_planner_B.e_on = 0;
11130 cartesian_trajectory_planner_B.d_f = 0;
11131 } else {
11132 cartesian_trajectory_planner_B.e_on = static_cast<int32_T>
11133 (cartesian_trajectory_planner_B.k) - 1;
11134 cartesian_trajectory_planner_B.d_f = static_cast<int32_T>
11135 (cartesian_trajectory_planner_B.n - 1.0);
11136 }
11137
11138 cartesian_trajectory_planner_B.b_kstr_i = switch_expression->size[0] *
11139 switch_expression->size[1];
11140 switch_expression->size[0] = 1;
11141 switch_expression->size[1] = body->JointInternal.Type->size[1];
11142 cartes_emxEnsureCapacity_char_T(switch_expression,
11143 cartesian_trajectory_planner_B.b_kstr_i);
11144 cartesian_trajectory_planner_B.loop_ub_e0 = body->JointInternal.Type->size[0]
11145 * body->JointInternal.Type->size[1] - 1;
11146 for (cartesian_trajectory_planner_B.b_kstr_i = 0;
11147 cartesian_trajectory_planner_B.b_kstr_i <=
11148 cartesian_trajectory_planner_B.loop_ub_e0;
11149 cartesian_trajectory_planner_B.b_kstr_i++) {
11150 switch_expression->data[cartesian_trajectory_planner_B.b_kstr_i] =
11151 body->JointInternal.Type->data[cartesian_trajectory_planner_B.b_kstr_i];
11152 }
11153
11154 cartesian_trajectory_planner_B.b_bool_c = false;
11155 if (switch_expression->size[1] == 5) {
11156 cartesian_trajectory_planner_B.b_kstr_i = 1;
11157 do {
11158 exitg1 = 0;
11159 if (cartesian_trajectory_planner_B.b_kstr_i - 1 < 5) {
11160 cartesian_trajectory_planner_B.loop_ub_e0 =
11161 cartesian_trajectory_planner_B.b_kstr_i - 1;
11162 if (switch_expression->data[cartesian_trajectory_planner_B.loop_ub_e0]
11163 !=
11164 cartesian_trajectory_planner_B.b_cf[cartesian_trajectory_planner_B.loop_ub_e0])
11165 {
11166 exitg1 = 1;
11167 } else {
11168 cartesian_trajectory_planner_B.b_kstr_i++;
11169 }
11170 } else {
11171 cartesian_trajectory_planner_B.b_bool_c = true;
11172 exitg1 = 1;
11173 }
11174 } while (exitg1 == 0);
11175 }
11176
11177 if (cartesian_trajectory_planner_B.b_bool_c) {
11178 cartesian_trajectory_planner_B.b_kstr_i = 0;
11179 } else {
11180 for (cartesian_trajectory_planner_B.b_kstr_i = 0;
11181 cartesian_trajectory_planner_B.b_kstr_i < 8;
11182 cartesian_trajectory_planner_B.b_kstr_i++) {
11183 cartesian_trajectory_planner_B.b_k[cartesian_trajectory_planner_B.b_kstr_i]
11184 = tmp_1[cartesian_trajectory_planner_B.b_kstr_i];
11185 }
11186
11187 cartesian_trajectory_planner_B.b_bool_c = false;
11188 if (switch_expression->size[1] == 8) {
11189 cartesian_trajectory_planner_B.b_kstr_i = 1;
11190 do {
11191 exitg1 = 0;
11192 if (cartesian_trajectory_planner_B.b_kstr_i - 1 < 8) {
11193 cartesian_trajectory_planner_B.loop_ub_e0 =
11194 cartesian_trajectory_planner_B.b_kstr_i - 1;
11195 if (switch_expression->
11196 data[cartesian_trajectory_planner_B.loop_ub_e0] !=
11197 cartesian_trajectory_planner_B.b_k[cartesian_trajectory_planner_B.loop_ub_e0])
11198 {
11199 exitg1 = 1;
11200 } else {
11201 cartesian_trajectory_planner_B.b_kstr_i++;
11202 }
11203 } else {
11204 cartesian_trajectory_planner_B.b_bool_c = true;
11205 exitg1 = 1;
11206 }
11207 } while (exitg1 == 0);
11208 }
11209
11210 if (cartesian_trajectory_planner_B.b_bool_c) {
11211 cartesian_trajectory_planner_B.b_kstr_i = 1;
11212 } else {
11213 cartesian_trajectory_planner_B.b_kstr_i = -1;
11214 }
11215 }
11216
11217 switch (cartesian_trajectory_planner_B.b_kstr_i) {
11218 case 0:
11219 memset(&cartesian_trajectory_planner_B.c_f1[0], 0, sizeof(real_T) << 4U);
11220 cartesian_trajectory_planner_B.c_f1[0] = 1.0;
11221 cartesian_trajectory_planner_B.c_f1[5] = 1.0;
11222 cartesian_trajectory_planner_B.c_f1[10] = 1.0;
11223 cartesian_trajectory_planner_B.c_f1[15] = 1.0;
11224 break;
11225
11226 case 1:
11227 ca_rigidBodyJoint_get_JointAxis(&body->JointInternal,
11228 cartesian_trajectory_planner_B.v_i);
11229 cartesian_trajectory_planner_B.d_f -= cartesian_trajectory_planner_B.e_on;
11230 for (cartesian_trajectory_planner_B.b_kstr_i = 0;
11231 cartesian_trajectory_planner_B.b_kstr_i <
11232 cartesian_trajectory_planner_B.d_f;
11233 cartesian_trajectory_planner_B.b_kstr_i++) {
11234 cartesian_trajectory_planner_B.e_data[cartesian_trajectory_planner_B.b_kstr_i]
11235 = cartesian_trajectory_planner_B.e_on +
11236 cartesian_trajectory_planner_B.b_kstr_i;
11237 }
11238
11239 cartesian_trajectory_planner_B.result_data_c[0] =
11240 cartesian_trajectory_planner_B.v_i[0];
11241 cartesian_trajectory_planner_B.result_data_c[1] =
11242 cartesian_trajectory_planner_B.v_i[1];
11243 cartesian_trajectory_planner_B.result_data_c[2] =
11244 cartesian_trajectory_planner_B.v_i[2];
11245 if (0 <= (cartesian_trajectory_planner_B.d_f != 0) - 1) {
11246 cartesian_trajectory_planner_B.result_data_c[3] =
11247 qvec[cartesian_trajectory_planner_B.e_data[0]];
11248 }
11249
11250 cartesian_trajectory_planner_B.k = 1.0 / sqrt
11251 ((cartesian_trajectory_planner_B.result_data_c[0] *
11252 cartesian_trajectory_planner_B.result_data_c[0] +
11253 cartesian_trajectory_planner_B.result_data_c[1] *
11254 cartesian_trajectory_planner_B.result_data_c[1]) +
11255 cartesian_trajectory_planner_B.result_data_c[2] *
11256 cartesian_trajectory_planner_B.result_data_c[2]);
11257 cartesian_trajectory_planner_B.v_i[0] =
11258 cartesian_trajectory_planner_B.result_data_c[0] *
11259 cartesian_trajectory_planner_B.k;
11260 cartesian_trajectory_planner_B.v_i[1] =
11261 cartesian_trajectory_planner_B.result_data_c[1] *
11262 cartesian_trajectory_planner_B.k;
11263 cartesian_trajectory_planner_B.v_i[2] =
11264 cartesian_trajectory_planner_B.result_data_c[2] *
11265 cartesian_trajectory_planner_B.k;
11266 cartesian_trajectory_planner_B.k = cos
11267 (cartesian_trajectory_planner_B.result_data_c[3]);
11268 cartesian_trajectory_planner_B.sth_c = sin
11269 (cartesian_trajectory_planner_B.result_data_c[3]);
11270 cartesian_trajectory_planner_B.tempR_m[0] =
11271 cartesian_trajectory_planner_B.v_i[0] *
11272 cartesian_trajectory_planner_B.v_i[0] * (1.0 -
11273 cartesian_trajectory_planner_B.k) + cartesian_trajectory_planner_B.k;
11274 cartesian_trajectory_planner_B.tempR_tmp_p =
11275 cartesian_trajectory_planner_B.v_i[1] *
11276 cartesian_trajectory_planner_B.v_i[0] * (1.0 -
11277 cartesian_trajectory_planner_B.k);
11278 cartesian_trajectory_planner_B.tempR_tmp_pi =
11279 cartesian_trajectory_planner_B.v_i[2] *
11280 cartesian_trajectory_planner_B.sth_c;
11281 cartesian_trajectory_planner_B.tempR_m[1] =
11282 cartesian_trajectory_planner_B.tempR_tmp_p -
11283 cartesian_trajectory_planner_B.tempR_tmp_pi;
11284 cartesian_trajectory_planner_B.tempR_tmp_a =
11285 cartesian_trajectory_planner_B.v_i[2] *
11286 cartesian_trajectory_planner_B.v_i[0] * (1.0 -
11287 cartesian_trajectory_planner_B.k);
11288 cartesian_trajectory_planner_B.tempR_tmp_ow =
11289 cartesian_trajectory_planner_B.v_i[1] *
11290 cartesian_trajectory_planner_B.sth_c;
11291 cartesian_trajectory_planner_B.tempR_m[2] =
11292 cartesian_trajectory_planner_B.tempR_tmp_a +
11293 cartesian_trajectory_planner_B.tempR_tmp_ow;
11294 cartesian_trajectory_planner_B.tempR_m[3] =
11295 cartesian_trajectory_planner_B.tempR_tmp_p +
11296 cartesian_trajectory_planner_B.tempR_tmp_pi;
11297 cartesian_trajectory_planner_B.tempR_m[4] =
11298 cartesian_trajectory_planner_B.v_i[1] *
11299 cartesian_trajectory_planner_B.v_i[1] * (1.0 -
11300 cartesian_trajectory_planner_B.k) + cartesian_trajectory_planner_B.k;
11301 cartesian_trajectory_planner_B.tempR_tmp_p =
11302 cartesian_trajectory_planner_B.v_i[2] *
11303 cartesian_trajectory_planner_B.v_i[1] * (1.0 -
11304 cartesian_trajectory_planner_B.k);
11305 cartesian_trajectory_planner_B.tempR_tmp_pi =
11306 cartesian_trajectory_planner_B.v_i[0] *
11307 cartesian_trajectory_planner_B.sth_c;
11308 cartesian_trajectory_planner_B.tempR_m[5] =
11309 cartesian_trajectory_planner_B.tempR_tmp_p -
11310 cartesian_trajectory_planner_B.tempR_tmp_pi;
11311 cartesian_trajectory_planner_B.tempR_m[6] =
11312 cartesian_trajectory_planner_B.tempR_tmp_a -
11313 cartesian_trajectory_planner_B.tempR_tmp_ow;
11314 cartesian_trajectory_planner_B.tempR_m[7] =
11315 cartesian_trajectory_planner_B.tempR_tmp_p +
11316 cartesian_trajectory_planner_B.tempR_tmp_pi;
11317 cartesian_trajectory_planner_B.tempR_m[8] =
11318 cartesian_trajectory_planner_B.v_i[2] *
11319 cartesian_trajectory_planner_B.v_i[2] * (1.0 -
11320 cartesian_trajectory_planner_B.k) + cartesian_trajectory_planner_B.k;
11321 for (cartesian_trajectory_planner_B.b_kstr_i = 0;
11322 cartesian_trajectory_planner_B.b_kstr_i < 3;
11323 cartesian_trajectory_planner_B.b_kstr_i++) {
11324 cartesian_trajectory_planner_B.e_on =
11325 cartesian_trajectory_planner_B.b_kstr_i + 1;
11326 cartesian_trajectory_planner_B.R_m[cartesian_trajectory_planner_B.e_on -
11327 1] = cartesian_trajectory_planner_B.tempR_m
11328 [(cartesian_trajectory_planner_B.e_on - 1) * 3];
11329 cartesian_trajectory_planner_B.e_on =
11330 cartesian_trajectory_planner_B.b_kstr_i + 1;
11331 cartesian_trajectory_planner_B.R_m[cartesian_trajectory_planner_B.e_on +
11332 2] = cartesian_trajectory_planner_B.tempR_m
11333 [(cartesian_trajectory_planner_B.e_on - 1) * 3 + 1];
11334 cartesian_trajectory_planner_B.e_on =
11335 cartesian_trajectory_planner_B.b_kstr_i + 1;
11336 cartesian_trajectory_planner_B.R_m[cartesian_trajectory_planner_B.e_on +
11337 5] = cartesian_trajectory_planner_B.tempR_m
11338 [(cartesian_trajectory_planner_B.e_on - 1) * 3 + 2];
11339 }
11340
11341 memset(&cartesian_trajectory_planner_B.c_f1[0], 0, sizeof(real_T) << 4U);
11342 for (cartesian_trajectory_planner_B.b_kstr_i = 0;
11343 cartesian_trajectory_planner_B.b_kstr_i < 3;
11344 cartesian_trajectory_planner_B.b_kstr_i++) {
11345 cartesian_trajectory_planner_B.d_f =
11346 cartesian_trajectory_planner_B.b_kstr_i << 2;
11347 cartesian_trajectory_planner_B.c_f1[cartesian_trajectory_planner_B.d_f] =
11348 cartesian_trajectory_planner_B.R_m[3 *
11349 cartesian_trajectory_planner_B.b_kstr_i];
11350 cartesian_trajectory_planner_B.c_f1[cartesian_trajectory_planner_B.d_f +
11351 1] = cartesian_trajectory_planner_B.R_m[3 *
11352 cartesian_trajectory_planner_B.b_kstr_i + 1];
11353 cartesian_trajectory_planner_B.c_f1[cartesian_trajectory_planner_B.d_f +
11354 2] = cartesian_trajectory_planner_B.R_m[3 *
11355 cartesian_trajectory_planner_B.b_kstr_i + 2];
11356 }
11357
11358 cartesian_trajectory_planner_B.c_f1[15] = 1.0;
11359 break;
11360
11361 default:
11362 ca_rigidBodyJoint_get_JointAxis(&body->JointInternal,
11363 cartesian_trajectory_planner_B.v_i);
11364 memset(&cartesian_trajectory_planner_B.tempR_m[0], 0, 9U * sizeof(real_T));
11365 cartesian_trajectory_planner_B.tempR_m[0] = 1.0;
11366 cartesian_trajectory_planner_B.tempR_m[4] = 1.0;
11367 cartesian_trajectory_planner_B.tempR_m[8] = 1.0;
11368 for (cartesian_trajectory_planner_B.b_kstr_i = 0;
11369 cartesian_trajectory_planner_B.b_kstr_i < 3;
11370 cartesian_trajectory_planner_B.b_kstr_i++) {
11371 cartesian_trajectory_planner_B.d_f =
11372 cartesian_trajectory_planner_B.b_kstr_i << 2;
11373 cartesian_trajectory_planner_B.c_f1[cartesian_trajectory_planner_B.d_f] =
11374 cartesian_trajectory_planner_B.tempR_m[3 *
11375 cartesian_trajectory_planner_B.b_kstr_i];
11376 cartesian_trajectory_planner_B.c_f1[cartesian_trajectory_planner_B.d_f +
11377 1] = cartesian_trajectory_planner_B.tempR_m[3 *
11378 cartesian_trajectory_planner_B.b_kstr_i + 1];
11379 cartesian_trajectory_planner_B.c_f1[cartesian_trajectory_planner_B.d_f +
11380 2] = cartesian_trajectory_planner_B.tempR_m[3 *
11381 cartesian_trajectory_planner_B.b_kstr_i + 2];
11382 cartesian_trajectory_planner_B.c_f1[cartesian_trajectory_planner_B.b_kstr_i
11383 + 12] =
11384 cartesian_trajectory_planner_B.v_i[cartesian_trajectory_planner_B.b_kstr_i]
11385 * qvec[cartesian_trajectory_planner_B.e_on];
11386 }
11387
11388 cartesian_trajectory_planner_B.c_f1[3] = 0.0;
11389 cartesian_trajectory_planner_B.c_f1[7] = 0.0;
11390 cartesian_trajectory_planner_B.c_f1[11] = 0.0;
11391 cartesian_trajectory_planner_B.c_f1[15] = 1.0;
11392 break;
11393 }
11394
11395 for (cartesian_trajectory_planner_B.b_kstr_i = 0;
11396 cartesian_trajectory_planner_B.b_kstr_i < 16;
11397 cartesian_trajectory_planner_B.b_kstr_i++) {
11398 cartesian_trajectory_planner_B.a_l[cartesian_trajectory_planner_B.b_kstr_i]
11399 = body->
11400 JointInternal.JointToParentTransform[cartesian_trajectory_planner_B.b_kstr_i];
11401 }
11402
11403 for (cartesian_trajectory_planner_B.b_kstr_i = 0;
11404 cartesian_trajectory_planner_B.b_kstr_i < 16;
11405 cartesian_trajectory_planner_B.b_kstr_i++) {
11406 cartesian_trajectory_planner_B.b_o[cartesian_trajectory_planner_B.b_kstr_i]
11407 = body->
11408 JointInternal.ChildToJointTransform[cartesian_trajectory_planner_B.b_kstr_i];
11409 }
11410
11411 for (cartesian_trajectory_planner_B.b_kstr_i = 0;
11412 cartesian_trajectory_planner_B.b_kstr_i < 4;
11413 cartesian_trajectory_planner_B.b_kstr_i++) {
11414 for (cartesian_trajectory_planner_B.e_on = 0;
11415 cartesian_trajectory_planner_B.e_on < 4;
11416 cartesian_trajectory_planner_B.e_on++) {
11417 cartesian_trajectory_planner_B.d_f = cartesian_trajectory_planner_B.e_on
11418 << 2;
11419 cartesian_trajectory_planner_B.loop_ub_e0 =
11420 cartesian_trajectory_planner_B.b_kstr_i +
11421 cartesian_trajectory_planner_B.d_f;
11422 cartesian_trajectory_planner_B.a_b[cartesian_trajectory_planner_B.loop_ub_e0]
11423 = 0.0;
11424 cartesian_trajectory_planner_B.a_b[cartesian_trajectory_planner_B.loop_ub_e0]
11425 +=
11426 cartesian_trajectory_planner_B.c_f1[cartesian_trajectory_planner_B.d_f]
11427 * cartesian_trajectory_planner_B.a_l[cartesian_trajectory_planner_B.b_kstr_i];
11428 cartesian_trajectory_planner_B.a_b[cartesian_trajectory_planner_B.loop_ub_e0]
11429 +=
11430 cartesian_trajectory_planner_B.c_f1[cartesian_trajectory_planner_B.d_f
11431 + 1] *
11432 cartesian_trajectory_planner_B.a_l[cartesian_trajectory_planner_B.b_kstr_i
11433 + 4];
11434 cartesian_trajectory_planner_B.a_b[cartesian_trajectory_planner_B.loop_ub_e0]
11435 +=
11436 cartesian_trajectory_planner_B.c_f1[cartesian_trajectory_planner_B.d_f
11437 + 2] *
11438 cartesian_trajectory_planner_B.a_l[cartesian_trajectory_planner_B.b_kstr_i
11439 + 8];
11440 cartesian_trajectory_planner_B.a_b[cartesian_trajectory_planner_B.loop_ub_e0]
11441 +=
11442 cartesian_trajectory_planner_B.c_f1[cartesian_trajectory_planner_B.d_f
11443 + 3] *
11444 cartesian_trajectory_planner_B.a_l[cartesian_trajectory_planner_B.b_kstr_i
11445 + 12];
11446 }
11447
11448 for (cartesian_trajectory_planner_B.e_on = 0;
11449 cartesian_trajectory_planner_B.e_on < 4;
11450 cartesian_trajectory_planner_B.e_on++) {
11451 cartesian_trajectory_planner_B.d_f = cartesian_trajectory_planner_B.e_on
11452 << 2;
11453 cartesian_trajectory_planner_B.loop_ub_e0 =
11454 cartesian_trajectory_planner_B.b_kstr_i +
11455 cartesian_trajectory_planner_B.d_f;
11456 Ttree->data[cartesian_trajectory_planner_B.b_jtilecol]
11457 .f1[cartesian_trajectory_planner_B.loop_ub_e0] = 0.0;
11458 Ttree->data[cartesian_trajectory_planner_B.b_jtilecol]
11459 .f1[cartesian_trajectory_planner_B.loop_ub_e0] +=
11460 cartesian_trajectory_planner_B.b_o[cartesian_trajectory_planner_B.d_f]
11461 * cartesian_trajectory_planner_B.a_b[cartesian_trajectory_planner_B.b_kstr_i];
11462 Ttree->data[cartesian_trajectory_planner_B.b_jtilecol]
11463 .f1[cartesian_trajectory_planner_B.loop_ub_e0] +=
11464 cartesian_trajectory_planner_B.b_o[cartesian_trajectory_planner_B.d_f
11465 + 1] *
11466 cartesian_trajectory_planner_B.a_b[cartesian_trajectory_planner_B.b_kstr_i
11467 + 4];
11468 Ttree->data[cartesian_trajectory_planner_B.b_jtilecol]
11469 .f1[cartesian_trajectory_planner_B.loop_ub_e0] +=
11470 cartesian_trajectory_planner_B.b_o[cartesian_trajectory_planner_B.d_f
11471 + 2] *
11472 cartesian_trajectory_planner_B.a_b[cartesian_trajectory_planner_B.b_kstr_i
11473 + 8];
11474 Ttree->data[cartesian_trajectory_planner_B.b_jtilecol]
11475 .f1[cartesian_trajectory_planner_B.loop_ub_e0] +=
11476 cartesian_trajectory_planner_B.b_o[cartesian_trajectory_planner_B.d_f
11477 + 3] *
11478 cartesian_trajectory_planner_B.a_b[cartesian_trajectory_planner_B.b_kstr_i
11479 + 12];
11480 }
11481 }
11482
11483 cartesian_trajectory_planner_B.k = cartesian_trajectory_planner_B.n;
11484 if (body->ParentIndex > 0.0) {
11485 for (cartesian_trajectory_planner_B.b_kstr_i = 0;
11486 cartesian_trajectory_planner_B.b_kstr_i < 16;
11487 cartesian_trajectory_planner_B.b_kstr_i++) {
11488 cartesian_trajectory_planner_B.a_l[cartesian_trajectory_planner_B.b_kstr_i]
11489 = Ttree->data[static_cast<int32_T>(body->ParentIndex) - 1]
11490 .f1[cartesian_trajectory_planner_B.b_kstr_i];
11491 }
11492
11493 for (cartesian_trajectory_planner_B.b_kstr_i = 0;
11494 cartesian_trajectory_planner_B.b_kstr_i < 4;
11495 cartesian_trajectory_planner_B.b_kstr_i++) {
11496 for (cartesian_trajectory_planner_B.e_on = 0;
11497 cartesian_trajectory_planner_B.e_on < 4;
11498 cartesian_trajectory_planner_B.e_on++) {
11499 cartesian_trajectory_planner_B.d_f =
11500 cartesian_trajectory_planner_B.e_on << 2;
11501 cartesian_trajectory_planner_B.loop_ub_e0 =
11502 cartesian_trajectory_planner_B.b_kstr_i +
11503 cartesian_trajectory_planner_B.d_f;
11504 cartesian_trajectory_planner_B.a_b[cartesian_trajectory_planner_B.loop_ub_e0]
11505 = 0.0;
11506 cartesian_trajectory_planner_B.a_b[cartesian_trajectory_planner_B.loop_ub_e0]
11507 += Ttree->data[cartesian_trajectory_planner_B.b_jtilecol]
11508 .f1[cartesian_trajectory_planner_B.d_f] *
11509 cartesian_trajectory_planner_B.a_l[cartesian_trajectory_planner_B.b_kstr_i];
11510 cartesian_trajectory_planner_B.a_b[cartesian_trajectory_planner_B.loop_ub_e0]
11511 += Ttree->data[cartesian_trajectory_planner_B.b_jtilecol]
11512 .f1[cartesian_trajectory_planner_B.d_f + 1] *
11513 cartesian_trajectory_planner_B.a_l[cartesian_trajectory_planner_B.b_kstr_i
11514 + 4];
11515 cartesian_trajectory_planner_B.a_b[cartesian_trajectory_planner_B.loop_ub_e0]
11516 += Ttree->data[cartesian_trajectory_planner_B.b_jtilecol]
11517 .f1[cartesian_trajectory_planner_B.d_f + 2] *
11518 cartesian_trajectory_planner_B.a_l[cartesian_trajectory_planner_B.b_kstr_i
11519 + 8];
11520 cartesian_trajectory_planner_B.a_b[cartesian_trajectory_planner_B.loop_ub_e0]
11521 += Ttree->data[cartesian_trajectory_planner_B.b_jtilecol]
11522 .f1[cartesian_trajectory_planner_B.d_f + 3] *
11523 cartesian_trajectory_planner_B.a_l[cartesian_trajectory_planner_B.b_kstr_i
11524 + 12];
11525 }
11526 }
11527
11528 for (cartesian_trajectory_planner_B.b_kstr_i = 0;
11529 cartesian_trajectory_planner_B.b_kstr_i < 16;
11530 cartesian_trajectory_planner_B.b_kstr_i++) {
11531 Ttree->data[cartesian_trajectory_planner_B.b_jtilecol]
11532 .f1[cartesian_trajectory_planner_B.b_kstr_i] =
11533 cartesian_trajectory_planner_B.a_b[cartesian_trajectory_planner_B.b_kstr_i];
11534 }
11535 }
11536 }
11537
11538 cartesian_trajec_emxFree_char_T(&switch_expression);
11539}
11540
11541static void cartesian_t_emxFree_f_cell_wrap(emxArray_f_cell_wrap_cartesia_T
11542 **pEmxArray)
11543{
11544 if (*pEmxArray != (emxArray_f_cell_wrap_cartesia_T *)NULL) {
11545 if (((*pEmxArray)->data != (f_cell_wrap_cartesian_traject_T *)NULL) &&
11546 (*pEmxArray)->canFreeData) {
11547 free((*pEmxArray)->data);
11548 }
11549
11550 free((*pEmxArray)->size);
11551 free(*pEmxArray);
11552 *pEmxArray = (emxArray_f_cell_wrap_cartesia_T *)NULL;
11553 }
11554}
11555
11556static void RigidBodyTree_geometricJacobian(p_robotics_manip_internal_Rig_T *obj,
11557 const real_T Q[6], emxArray_real_T_cartesian_tra_T *Jac)
11558{
11559 emxArray_f_cell_wrap_cartesia_T *Ttree;
11560 n_robotics_manip_internal_Rig_T *body;
11561 emxArray_real_T_cartesian_tra_T *JacSlice;
11562 emxArray_char_T_cartesian_tra_T *bname;
11563 emxArray_real_T_cartesian_tra_T *b;
11564 static const char_T tmp[11] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
11565 'e', 'e' };
11566
11567 static const char_T tmp_0[5] = { 'f', 'i', 'x', 'e', 'd' };
11568
11569 int32_T exitg1;
11570 boolean_T exitg2;
11571 cartesian_t_emxInit_f_cell_wrap(&Ttree, 2);
11572 RigidBodyTree_forwardKinematics(obj, Q, Ttree);
11573 cartesian_trajectory_planner_B.b_kstr_lu = Jac->size[0] * Jac->size[1];
11574 Jac->size[0] = 6;
11575 Jac->size[1] = static_cast<int32_T>(obj->VelocityNumber);
11576 cartes_emxEnsureCapacity_real_T(Jac, cartesian_trajectory_planner_B.b_kstr_lu);
11577 cartesian_trajectory_planner_B.loop_ub_i = 6 * static_cast<int32_T>
11578 (obj->VelocityNumber) - 1;
11579 for (cartesian_trajectory_planner_B.b_kstr_lu = 0;
11580 cartesian_trajectory_planner_B.b_kstr_lu <=
11581 cartesian_trajectory_planner_B.loop_ub_i;
11582 cartesian_trajectory_planner_B.b_kstr_lu++) {
11583 Jac->data[cartesian_trajectory_planner_B.b_kstr_lu] = 0.0;
11584 }
11585
11586 for (cartesian_trajectory_planner_B.b_kstr_lu = 0;
11587 cartesian_trajectory_planner_B.b_kstr_lu < 8;
11588 cartesian_trajectory_planner_B.b_kstr_lu++) {
11589 cartesian_trajectory_planner_B.chainmask[cartesian_trajectory_planner_B.b_kstr_lu]
11590 = 0;
11591 }
11592
11593 cartesian_trajec_emxInit_char_T(&bname, 2);
11594 cartesian_trajectory_planner_B.b_kstr_lu = bname->size[0] * bname->size[1];
11595 bname->size[0] = 1;
11596 bname->size[1] = obj->Base.NameInternal->size[1];
11597 cartes_emxEnsureCapacity_char_T(bname,
11598 cartesian_trajectory_planner_B.b_kstr_lu);
11599 cartesian_trajectory_planner_B.loop_ub_i = obj->Base.NameInternal->size[0] *
11600 obj->Base.NameInternal->size[1] - 1;
11601 for (cartesian_trajectory_planner_B.b_kstr_lu = 0;
11602 cartesian_trajectory_planner_B.b_kstr_lu <=
11603 cartesian_trajectory_planner_B.loop_ub_i;
11604 cartesian_trajectory_planner_B.b_kstr_lu++) {
11605 bname->data[cartesian_trajectory_planner_B.b_kstr_lu] =
11606 obj->Base.NameInternal->data[cartesian_trajectory_planner_B.b_kstr_lu];
11607 }
11608
11609 for (cartesian_trajectory_planner_B.b_kstr_lu = 0;
11610 cartesian_trajectory_planner_B.b_kstr_lu < 11;
11611 cartesian_trajectory_planner_B.b_kstr_lu++) {
11612 cartesian_trajectory_planner_B.a_jg[cartesian_trajectory_planner_B.b_kstr_lu]
11613 = tmp[cartesian_trajectory_planner_B.b_kstr_lu];
11614 }
11615
11616 cartesian_trajectory_planner_B.b_bool_n = false;
11617 if (11 == bname->size[1]) {
11618 cartesian_trajectory_planner_B.b_kstr_lu = 1;
11619 do {
11620 exitg1 = 0;
11621 if (cartesian_trajectory_planner_B.b_kstr_lu - 1 < 11) {
11622 cartesian_trajectory_planner_B.kstr =
11623 cartesian_trajectory_planner_B.b_kstr_lu - 1;
11624 if (cartesian_trajectory_planner_B.a_jg[cartesian_trajectory_planner_B.kstr]
11625 != bname->data[cartesian_trajectory_planner_B.kstr]) {
11626 exitg1 = 1;
11627 } else {
11628 cartesian_trajectory_planner_B.b_kstr_lu++;
11629 }
11630 } else {
11631 cartesian_trajectory_planner_B.b_bool_n = true;
11632 exitg1 = 1;
11633 }
11634 } while (exitg1 == 0);
11635 }
11636
11637 if (cartesian_trajectory_planner_B.b_bool_n) {
11638 memset(&cartesian_trajectory_planner_B.T2inv[0], 0, sizeof(real_T) << 4U);
11639 cartesian_trajectory_planner_B.T2inv[0] = 1.0;
11640 cartesian_trajectory_planner_B.T2inv[5] = 1.0;
11641 cartesian_trajectory_planner_B.T2inv[10] = 1.0;
11642 cartesian_trajectory_planner_B.T2inv[15] = 1.0;
11643 memset(&cartesian_trajectory_planner_B.T2[0], 0, sizeof(real_T) << 4U);
11644 cartesian_trajectory_planner_B.T2[0] = 1.0;
11645 cartesian_trajectory_planner_B.T2[5] = 1.0;
11646 cartesian_trajectory_planner_B.T2[10] = 1.0;
11647 cartesian_trajectory_planner_B.T2[15] = 1.0;
11648 } else {
11649 cartesian_trajectory_planner_B.endeffectorIndex = -1.0;
11650 cartesian_trajectory_planner_B.b_kstr_lu = bname->size[0] * bname->size[1];
11651 bname->size[0] = 1;
11652 bname->size[1] = obj->Base.NameInternal->size[1];
11653 cartes_emxEnsureCapacity_char_T(bname,
11654 cartesian_trajectory_planner_B.b_kstr_lu);
11655 cartesian_trajectory_planner_B.loop_ub_i = obj->Base.NameInternal->size[0] *
11656 obj->Base.NameInternal->size[1] - 1;
11657 for (cartesian_trajectory_planner_B.b_kstr_lu = 0;
11658 cartesian_trajectory_planner_B.b_kstr_lu <=
11659 cartesian_trajectory_planner_B.loop_ub_i;
11660 cartesian_trajectory_planner_B.b_kstr_lu++) {
11661 bname->data[cartesian_trajectory_planner_B.b_kstr_lu] =
11662 obj->Base.NameInternal->data[cartesian_trajectory_planner_B.b_kstr_lu];
11663 }
11664
11665 cartesian_trajectory_planner_B.b_bool_n = false;
11666 if (bname->size[1] == 11) {
11667 cartesian_trajectory_planner_B.b_kstr_lu = 1;
11668 do {
11669 exitg1 = 0;
11670 if (cartesian_trajectory_planner_B.b_kstr_lu - 1 < 11) {
11671 cartesian_trajectory_planner_B.kstr =
11672 cartesian_trajectory_planner_B.b_kstr_lu - 1;
11673 if (bname->data[cartesian_trajectory_planner_B.kstr] !=
11674 cartesian_trajectory_planner_B.a_jg[cartesian_trajectory_planner_B.kstr])
11675 {
11676 exitg1 = 1;
11677 } else {
11678 cartesian_trajectory_planner_B.b_kstr_lu++;
11679 }
11680 } else {
11681 cartesian_trajectory_planner_B.b_bool_n = true;
11682 exitg1 = 1;
11683 }
11684 } while (exitg1 == 0);
11685 }
11686
11687 if (cartesian_trajectory_planner_B.b_bool_n) {
11688 cartesian_trajectory_planner_B.endeffectorIndex = 0.0;
11689 } else {
11690 cartesian_trajectory_planner_B.idx_idx_1 = obj->NumBodies;
11691 cartesian_trajectory_planner_B.b_i_a = 0;
11692 exitg2 = false;
11693 while ((!exitg2) && (cartesian_trajectory_planner_B.b_i_a <=
11694 static_cast<int32_T>
11695 (cartesian_trajectory_planner_B.idx_idx_1) - 1)) {
11696 body = obj->Bodies[cartesian_trajectory_planner_B.b_i_a];
11697 cartesian_trajectory_planner_B.b_kstr_lu = bname->size[0] * bname->size
11698 [1];
11699 bname->size[0] = 1;
11700 bname->size[1] = body->NameInternal->size[1];
11701 cartes_emxEnsureCapacity_char_T(bname,
11702 cartesian_trajectory_planner_B.b_kstr_lu);
11703 cartesian_trajectory_planner_B.loop_ub_i = body->NameInternal->size[0] *
11704 body->NameInternal->size[1] - 1;
11705 for (cartesian_trajectory_planner_B.b_kstr_lu = 0;
11706 cartesian_trajectory_planner_B.b_kstr_lu <=
11707 cartesian_trajectory_planner_B.loop_ub_i;
11708 cartesian_trajectory_planner_B.b_kstr_lu++) {
11709 bname->data[cartesian_trajectory_planner_B.b_kstr_lu] =
11710 body->NameInternal->data[cartesian_trajectory_planner_B.b_kstr_lu];
11711 }
11712
11713 for (cartesian_trajectory_planner_B.b_kstr_lu = 0;
11714 cartesian_trajectory_planner_B.b_kstr_lu < 11;
11715 cartesian_trajectory_planner_B.b_kstr_lu++) {
11716 cartesian_trajectory_planner_B.a_jg[cartesian_trajectory_planner_B.b_kstr_lu]
11717 = tmp[cartesian_trajectory_planner_B.b_kstr_lu];
11718 }
11719
11720 cartesian_trajectory_planner_B.b_bool_n = false;
11721 if (bname->size[1] == 11) {
11722 cartesian_trajectory_planner_B.b_kstr_lu = 1;
11723 do {
11724 exitg1 = 0;
11725 if (cartesian_trajectory_planner_B.b_kstr_lu - 1 < 11) {
11726 cartesian_trajectory_planner_B.kstr =
11727 cartesian_trajectory_planner_B.b_kstr_lu - 1;
11728 if (bname->data[cartesian_trajectory_planner_B.kstr] !=
11729 cartesian_trajectory_planner_B.a_jg[cartesian_trajectory_planner_B.kstr])
11730 {
11731 exitg1 = 1;
11732 } else {
11733 cartesian_trajectory_planner_B.b_kstr_lu++;
11734 }
11735 } else {
11736 cartesian_trajectory_planner_B.b_bool_n = true;
11737 exitg1 = 1;
11738 }
11739 } while (exitg1 == 0);
11740 }
11741
11742 if (cartesian_trajectory_planner_B.b_bool_n) {
11743 cartesian_trajectory_planner_B.endeffectorIndex = static_cast<real_T>
11744 (cartesian_trajectory_planner_B.b_i_a) + 1.0;
11745 exitg2 = true;
11746 } else {
11747 cartesian_trajectory_planner_B.b_i_a++;
11748 }
11749 }
11750 }
11751
11752 cartesian_trajectory_planner_B.b_i_a = static_cast<int32_T>
11753 (cartesian_trajectory_planner_B.endeffectorIndex) - 1;
11754 body = obj->Bodies[cartesian_trajectory_planner_B.b_i_a];
11755 for (cartesian_trajectory_planner_B.b_kstr_lu = 0;
11756 cartesian_trajectory_planner_B.b_kstr_lu < 16;
11757 cartesian_trajectory_planner_B.b_kstr_lu++) {
11758 cartesian_trajectory_planner_B.T2[cartesian_trajectory_planner_B.b_kstr_lu]
11759 = Ttree->data[cartesian_trajectory_planner_B.b_i_a]
11760 .f1[cartesian_trajectory_planner_B.b_kstr_lu];
11761 }
11762
11763 for (cartesian_trajectory_planner_B.b_kstr_lu = 0;
11764 cartesian_trajectory_planner_B.b_kstr_lu < 3;
11765 cartesian_trajectory_planner_B.b_kstr_lu++) {
11766 cartesian_trajectory_planner_B.R_o[3 *
11767 cartesian_trajectory_planner_B.b_kstr_lu] = Ttree->
11768 data[cartesian_trajectory_planner_B.b_i_a]
11769 .f1[cartesian_trajectory_planner_B.b_kstr_lu];
11770 cartesian_trajectory_planner_B.R_o[3 *
11771 cartesian_trajectory_planner_B.b_kstr_lu + 1] = Ttree->
11772 data[cartesian_trajectory_planner_B.b_i_a]
11773 .f1[cartesian_trajectory_planner_B.b_kstr_lu + 4];
11774 cartesian_trajectory_planner_B.R_o[3 *
11775 cartesian_trajectory_planner_B.b_kstr_lu + 2] = Ttree->
11776 data[cartesian_trajectory_planner_B.b_i_a]
11777 .f1[cartesian_trajectory_planner_B.b_kstr_lu + 8];
11778 }
11779
11780 for (cartesian_trajectory_planner_B.b_kstr_lu = 0;
11781 cartesian_trajectory_planner_B.b_kstr_lu < 9;
11782 cartesian_trajectory_planner_B.b_kstr_lu++) {
11783 cartesian_trajectory_planner_B.R_l[cartesian_trajectory_planner_B.b_kstr_lu]
11784 =
11785 -cartesian_trajectory_planner_B.R_o[cartesian_trajectory_planner_B.b_kstr_lu];
11786 }
11787
11788 for (cartesian_trajectory_planner_B.b_kstr_lu = 0;
11789 cartesian_trajectory_planner_B.b_kstr_lu < 3;
11790 cartesian_trajectory_planner_B.b_kstr_lu++) {
11791 cartesian_trajectory_planner_B.endeffectorIndex = Ttree->
11792 data[cartesian_trajectory_planner_B.b_i_a].f1[12] *
11793 cartesian_trajectory_planner_B.R_l[cartesian_trajectory_planner_B.b_kstr_lu];
11794 cartesian_trajectory_planner_B.loop_ub_i =
11795 cartesian_trajectory_planner_B.b_kstr_lu << 2;
11796 cartesian_trajectory_planner_B.T2inv[cartesian_trajectory_planner_B.loop_ub_i]
11797 = cartesian_trajectory_planner_B.R_o[3 *
11798 cartesian_trajectory_planner_B.b_kstr_lu];
11799 cartesian_trajectory_planner_B.endeffectorIndex +=
11800 cartesian_trajectory_planner_B.R_l[cartesian_trajectory_planner_B.b_kstr_lu
11801 + 3] * Ttree->data[cartesian_trajectory_planner_B.b_i_a].f1[13];
11802 cartesian_trajectory_planner_B.T2inv[cartesian_trajectory_planner_B.loop_ub_i
11803 + 1] = cartesian_trajectory_planner_B.R_o[3 *
11804 cartesian_trajectory_planner_B.b_kstr_lu + 1];
11805 cartesian_trajectory_planner_B.endeffectorIndex +=
11806 cartesian_trajectory_planner_B.R_l[cartesian_trajectory_planner_B.b_kstr_lu
11807 + 6] * Ttree->data[cartesian_trajectory_planner_B.b_i_a].f1[14];
11808 cartesian_trajectory_planner_B.T2inv[cartesian_trajectory_planner_B.loop_ub_i
11809 + 2] = cartesian_trajectory_planner_B.R_o[3 *
11810 cartesian_trajectory_planner_B.b_kstr_lu + 2];
11811 cartesian_trajectory_planner_B.T2inv[cartesian_trajectory_planner_B.b_kstr_lu
11812 + 12] = cartesian_trajectory_planner_B.endeffectorIndex;
11813 }
11814
11815 cartesian_trajectory_planner_B.T2inv[3] = 0.0;
11816 cartesian_trajectory_planner_B.T2inv[7] = 0.0;
11817 cartesian_trajectory_planner_B.T2inv[11] = 0.0;
11818 cartesian_trajectory_planner_B.T2inv[15] = 1.0;
11819 cartesian_trajectory_planner_B.chainmask[cartesian_trajectory_planner_B.b_i_a]
11820 = 1;
11821 while (body->ParentIndex > 0.0) {
11822 body = obj->Bodies[static_cast<int32_T>(body->ParentIndex) - 1];
11823 cartesian_trajectory_planner_B.chainmask[static_cast<int32_T>(body->Index)
11824 - 1] = 1;
11825 }
11826 }
11827
11828 cartesian_trajectory_planner_B.idx_idx_1 = obj->NumBodies;
11829 cartesian_trajectory_planner_B.c_c2 = static_cast<int32_T>
11830 (cartesian_trajectory_planner_B.idx_idx_1) - 1;
11831 cartesian_trajec_emxInit_real_T(&JacSlice, 2);
11832 cartesian_trajec_emxInit_real_T(&b, 2);
11833 if (0 <= cartesian_trajectory_planner_B.c_c2) {
11834 for (cartesian_trajectory_planner_B.b_kstr_lu = 0;
11835 cartesian_trajectory_planner_B.b_kstr_lu < 5;
11836 cartesian_trajectory_planner_B.b_kstr_lu++) {
11837 cartesian_trajectory_planner_B.b_h[cartesian_trajectory_planner_B.b_kstr_lu]
11838 = tmp_0[cartesian_trajectory_planner_B.b_kstr_lu];
11839 }
11840 }
11841
11842 for (cartesian_trajectory_planner_B.b_i_a = 0;
11843 cartesian_trajectory_planner_B.b_i_a <=
11844 cartesian_trajectory_planner_B.c_c2; cartesian_trajectory_planner_B.b_i_a
11845 ++) {
11846 body = obj->Bodies[cartesian_trajectory_planner_B.b_i_a];
11847 cartesian_trajectory_planner_B.b_kstr_lu = bname->size[0] * bname->size[1];
11848 bname->size[0] = 1;
11849 bname->size[1] = body->JointInternal.Type->size[1];
11850 cartes_emxEnsureCapacity_char_T(bname,
11851 cartesian_trajectory_planner_B.b_kstr_lu);
11852 cartesian_trajectory_planner_B.loop_ub_i = body->JointInternal.Type->size[0]
11853 * body->JointInternal.Type->size[1] - 1;
11854 for (cartesian_trajectory_planner_B.b_kstr_lu = 0;
11855 cartesian_trajectory_planner_B.b_kstr_lu <=
11856 cartesian_trajectory_planner_B.loop_ub_i;
11857 cartesian_trajectory_planner_B.b_kstr_lu++) {
11858 bname->data[cartesian_trajectory_planner_B.b_kstr_lu] =
11859 body->JointInternal.Type->data[cartesian_trajectory_planner_B.b_kstr_lu];
11860 }
11861
11862 cartesian_trajectory_planner_B.b_bool_n = false;
11863 if (bname->size[1] == 5) {
11864 cartesian_trajectory_planner_B.b_kstr_lu = 1;
11865 do {
11866 exitg1 = 0;
11867 if (cartesian_trajectory_planner_B.b_kstr_lu - 1 < 5) {
11868 cartesian_trajectory_planner_B.kstr =
11869 cartesian_trajectory_planner_B.b_kstr_lu - 1;
11870 if (bname->data[cartesian_trajectory_planner_B.kstr] !=
11871 cartesian_trajectory_planner_B.b_h[cartesian_trajectory_planner_B.kstr])
11872 {
11873 exitg1 = 1;
11874 } else {
11875 cartesian_trajectory_planner_B.b_kstr_lu++;
11876 }
11877 } else {
11878 cartesian_trajectory_planner_B.b_bool_n = true;
11879 exitg1 = 1;
11880 }
11881 } while (exitg1 == 0);
11882 }
11883
11884 if ((!cartesian_trajectory_planner_B.b_bool_n) &&
11885 (cartesian_trajectory_planner_B.chainmask[cartesian_trajectory_planner_B.b_i_a]
11886 != 0)) {
11887 for (cartesian_trajectory_planner_B.b_kstr_lu = 0;
11888 cartesian_trajectory_planner_B.b_kstr_lu < 16;
11889 cartesian_trajectory_planner_B.b_kstr_lu++) {
11890 cartesian_trajectory_planner_B.T1_d[cartesian_trajectory_planner_B.b_kstr_lu]
11891 = Ttree->data[static_cast<int32_T>(body->Index) - 1]
11892 .f1[cartesian_trajectory_planner_B.b_kstr_lu];
11893 }
11894
11895 for (cartesian_trajectory_planner_B.b_kstr_lu = 0;
11896 cartesian_trajectory_planner_B.b_kstr_lu < 16;
11897 cartesian_trajectory_planner_B.b_kstr_lu++) {
11898 cartesian_trajectory_planner_B.Tdh[cartesian_trajectory_planner_B.b_kstr_lu]
11899 = body->
11900 JointInternal.ChildToJointTransform[cartesian_trajectory_planner_B.b_kstr_lu];
11901 }
11902
11903 for (cartesian_trajectory_planner_B.b_kstr_lu = 0;
11904 cartesian_trajectory_planner_B.b_kstr_lu < 3;
11905 cartesian_trajectory_planner_B.b_kstr_lu++) {
11906 cartesian_trajectory_planner_B.R_o[3 *
11907 cartesian_trajectory_planner_B.b_kstr_lu] =
11908 cartesian_trajectory_planner_B.Tdh[cartesian_trajectory_planner_B.b_kstr_lu];
11909 cartesian_trajectory_planner_B.R_o[3 *
11910 cartesian_trajectory_planner_B.b_kstr_lu + 1] =
11911 cartesian_trajectory_planner_B.Tdh[cartesian_trajectory_planner_B.b_kstr_lu
11912 + 4];
11913 cartesian_trajectory_planner_B.R_o[3 *
11914 cartesian_trajectory_planner_B.b_kstr_lu + 2] =
11915 cartesian_trajectory_planner_B.Tdh[cartesian_trajectory_planner_B.b_kstr_lu
11916 + 8];
11917 }
11918
11919 for (cartesian_trajectory_planner_B.b_kstr_lu = 0;
11920 cartesian_trajectory_planner_B.b_kstr_lu < 9;
11921 cartesian_trajectory_planner_B.b_kstr_lu++) {
11922 cartesian_trajectory_planner_B.R_l[cartesian_trajectory_planner_B.b_kstr_lu]
11923 =
11924 -cartesian_trajectory_planner_B.R_o[cartesian_trajectory_planner_B.b_kstr_lu];
11925 }
11926
11927 for (cartesian_trajectory_planner_B.b_kstr_lu = 0;
11928 cartesian_trajectory_planner_B.b_kstr_lu < 3;
11929 cartesian_trajectory_planner_B.b_kstr_lu++) {
11930 cartesian_trajectory_planner_B.R_n[cartesian_trajectory_planner_B.b_kstr_lu]
11931 =
11932 cartesian_trajectory_planner_B.R_l[cartesian_trajectory_planner_B.b_kstr_lu
11933 + 6] * cartesian_trajectory_planner_B.Tdh[14] +
11934 (cartesian_trajectory_planner_B.R_l[cartesian_trajectory_planner_B.b_kstr_lu
11935 + 3] * cartesian_trajectory_planner_B.Tdh[13] +
11936 cartesian_trajectory_planner_B.R_l[cartesian_trajectory_planner_B.b_kstr_lu]
11937 * cartesian_trajectory_planner_B.Tdh[12]);
11938 }
11939
11940 for (cartesian_trajectory_planner_B.b_kstr_lu = 0;
11941 cartesian_trajectory_planner_B.b_kstr_lu < 4;
11942 cartesian_trajectory_planner_B.b_kstr_lu++) {
11943 for (cartesian_trajectory_planner_B.kstr = 0;
11944 cartesian_trajectory_planner_B.kstr < 4;
11945 cartesian_trajectory_planner_B.kstr++) {
11946 cartesian_trajectory_planner_B.n_j =
11947 cartesian_trajectory_planner_B.kstr << 2;
11948 cartesian_trajectory_planner_B.loop_ub_i =
11949 cartesian_trajectory_planner_B.b_kstr_lu +
11950 cartesian_trajectory_planner_B.n_j;
11951 cartesian_trajectory_planner_B.Tdh[cartesian_trajectory_planner_B.loop_ub_i]
11952 = 0.0;
11953 cartesian_trajectory_planner_B.Tdh[cartesian_trajectory_planner_B.loop_ub_i]
11954 +=
11955 cartesian_trajectory_planner_B.T1_d[cartesian_trajectory_planner_B.n_j]
11956 * cartesian_trajectory_planner_B.T2inv[cartesian_trajectory_planner_B.b_kstr_lu];
11957 cartesian_trajectory_planner_B.Tdh[cartesian_trajectory_planner_B.loop_ub_i]
11958 +=
11959 cartesian_trajectory_planner_B.T1_d[cartesian_trajectory_planner_B.n_j
11960 + 1] *
11961 cartesian_trajectory_planner_B.T2inv[cartesian_trajectory_planner_B.b_kstr_lu
11962 + 4];
11963 cartesian_trajectory_planner_B.Tdh[cartesian_trajectory_planner_B.loop_ub_i]
11964 +=
11965 cartesian_trajectory_planner_B.T1_d[cartesian_trajectory_planner_B.n_j
11966 + 2] *
11967 cartesian_trajectory_planner_B.T2inv[cartesian_trajectory_planner_B.b_kstr_lu
11968 + 8];
11969 cartesian_trajectory_planner_B.Tdh[cartesian_trajectory_planner_B.loop_ub_i]
11970 +=
11971 cartesian_trajectory_planner_B.T1_d[cartesian_trajectory_planner_B.n_j
11972 + 3] *
11973 cartesian_trajectory_planner_B.T2inv[cartesian_trajectory_planner_B.b_kstr_lu
11974 + 12];
11975 }
11976 }
11977
11978 for (cartesian_trajectory_planner_B.b_kstr_lu = 0;
11979 cartesian_trajectory_planner_B.b_kstr_lu < 3;
11980 cartesian_trajectory_planner_B.b_kstr_lu++) {
11981 cartesian_trajectory_planner_B.kstr =
11982 cartesian_trajectory_planner_B.b_kstr_lu << 2;
11983 cartesian_trajectory_planner_B.T1_d[cartesian_trajectory_planner_B.kstr]
11984 = cartesian_trajectory_planner_B.R_o[3 *
11985 cartesian_trajectory_planner_B.b_kstr_lu];
11986 cartesian_trajectory_planner_B.T1_d[cartesian_trajectory_planner_B.kstr
11987 + 1] = cartesian_trajectory_planner_B.R_o[3 *
11988 cartesian_trajectory_planner_B.b_kstr_lu + 1];
11989 cartesian_trajectory_planner_B.T1_d[cartesian_trajectory_planner_B.kstr
11990 + 2] = cartesian_trajectory_planner_B.R_o[3 *
11991 cartesian_trajectory_planner_B.b_kstr_lu + 2];
11992 cartesian_trajectory_planner_B.T1_d[cartesian_trajectory_planner_B.b_kstr_lu
11993 + 12] =
11994 cartesian_trajectory_planner_B.R_n[cartesian_trajectory_planner_B.b_kstr_lu];
11995 }
11996
11997 cartesian_trajectory_planner_B.T1_d[3] = 0.0;
11998 cartesian_trajectory_planner_B.T1_d[7] = 0.0;
11999 cartesian_trajectory_planner_B.T1_d[11] = 0.0;
12000 cartesian_trajectory_planner_B.T1_d[15] = 1.0;
12001 for (cartesian_trajectory_planner_B.b_kstr_lu = 0;
12002 cartesian_trajectory_planner_B.b_kstr_lu < 4;
12003 cartesian_trajectory_planner_B.b_kstr_lu++) {
12004 for (cartesian_trajectory_planner_B.kstr = 0;
12005 cartesian_trajectory_planner_B.kstr < 4;
12006 cartesian_trajectory_planner_B.kstr++) {
12007 cartesian_trajectory_planner_B.loop_ub_i =
12008 cartesian_trajectory_planner_B.kstr << 2;
12009 cartesian_trajectory_planner_B.n_j =
12010 cartesian_trajectory_planner_B.b_kstr_lu +
12011 cartesian_trajectory_planner_B.loop_ub_i;
12012 cartesian_trajectory_planner_B.T[cartesian_trajectory_planner_B.n_j] =
12013 0.0;
12014 cartesian_trajectory_planner_B.T[cartesian_trajectory_planner_B.n_j] +=
12015 cartesian_trajectory_planner_B.T1_d[cartesian_trajectory_planner_B.loop_ub_i]
12016 * cartesian_trajectory_planner_B.Tdh[cartesian_trajectory_planner_B.b_kstr_lu];
12017 cartesian_trajectory_planner_B.T[cartesian_trajectory_planner_B.n_j] +=
12018 cartesian_trajectory_planner_B.T1_d[cartesian_trajectory_planner_B.loop_ub_i
12019 + 1] *
12020 cartesian_trajectory_planner_B.Tdh[cartesian_trajectory_planner_B.b_kstr_lu
12021 + 4];
12022 cartesian_trajectory_planner_B.T[cartesian_trajectory_planner_B.n_j] +=
12023 cartesian_trajectory_planner_B.T1_d[cartesian_trajectory_planner_B.loop_ub_i
12024 + 2] *
12025 cartesian_trajectory_planner_B.Tdh[cartesian_trajectory_planner_B.b_kstr_lu
12026 + 8];
12027 cartesian_trajectory_planner_B.T[cartesian_trajectory_planner_B.n_j] +=
12028 cartesian_trajectory_planner_B.T1_d[cartesian_trajectory_planner_B.loop_ub_i
12029 + 3] *
12030 cartesian_trajectory_planner_B.Tdh[cartesian_trajectory_planner_B.b_kstr_lu
12031 + 12];
12032 }
12033 }
12034
12035 cartesian_trajectory_planner_B.endeffectorIndex = obj->
12036 PositionDoFMap[cartesian_trajectory_planner_B.b_i_a];
12037 cartesian_trajectory_planner_B.idx_idx_1 = obj->
12038 PositionDoFMap[cartesian_trajectory_planner_B.b_i_a + 8];
12039 cartesian_trajectory_planner_B.R_o[0] = 0.0;
12040 cartesian_trajectory_planner_B.R_o[3] = -cartesian_trajectory_planner_B.T
12041 [14];
12042 cartesian_trajectory_planner_B.R_o[6] = cartesian_trajectory_planner_B.T
12043 [13];
12044 cartesian_trajectory_planner_B.R_o[1] = cartesian_trajectory_planner_B.T
12045 [14];
12046 cartesian_trajectory_planner_B.R_o[4] = 0.0;
12047 cartesian_trajectory_planner_B.R_o[7] = -cartesian_trajectory_planner_B.T
12048 [12];
12049 cartesian_trajectory_planner_B.R_o[2] = -cartesian_trajectory_planner_B.T
12050 [13];
12051 cartesian_trajectory_planner_B.R_o[5] = cartesian_trajectory_planner_B.T
12052 [12];
12053 cartesian_trajectory_planner_B.R_o[8] = 0.0;
12054 for (cartesian_trajectory_planner_B.b_kstr_lu = 0;
12055 cartesian_trajectory_planner_B.b_kstr_lu < 3;
12056 cartesian_trajectory_planner_B.b_kstr_lu++) {
12057 for (cartesian_trajectory_planner_B.kstr = 0;
12058 cartesian_trajectory_planner_B.kstr < 3;
12059 cartesian_trajectory_planner_B.kstr++) {
12060 cartesian_trajectory_planner_B.loop_ub_i =
12061 cartesian_trajectory_planner_B.b_kstr_lu + 3 *
12062 cartesian_trajectory_planner_B.kstr;
12063 cartesian_trajectory_planner_B.R_l[cartesian_trajectory_planner_B.loop_ub_i]
12064 = 0.0;
12065 cartesian_trajectory_planner_B.n_j =
12066 cartesian_trajectory_planner_B.kstr << 2;
12067 cartesian_trajectory_planner_B.R_l[cartesian_trajectory_planner_B.loop_ub_i]
12068 +=
12069 cartesian_trajectory_planner_B.T[cartesian_trajectory_planner_B.n_j]
12070 * cartesian_trajectory_planner_B.R_o[cartesian_trajectory_planner_B.b_kstr_lu];
12071 cartesian_trajectory_planner_B.R_l[cartesian_trajectory_planner_B.loop_ub_i]
12072 +=
12073 cartesian_trajectory_planner_B.T[cartesian_trajectory_planner_B.n_j
12074 + 1] *
12075 cartesian_trajectory_planner_B.R_o[cartesian_trajectory_planner_B.b_kstr_lu
12076 + 3];
12077 cartesian_trajectory_planner_B.R_l[cartesian_trajectory_planner_B.loop_ub_i]
12078 +=
12079 cartesian_trajectory_planner_B.T[cartesian_trajectory_planner_B.n_j
12080 + 2] *
12081 cartesian_trajectory_planner_B.R_o[cartesian_trajectory_planner_B.b_kstr_lu
12082 + 6];
12083 cartesian_trajectory_planner_B.X_f[cartesian_trajectory_planner_B.kstr
12084 + 6 * cartesian_trajectory_planner_B.b_kstr_lu] =
12085 cartesian_trajectory_planner_B.T
12086 [(cartesian_trajectory_planner_B.b_kstr_lu << 2) +
12087 cartesian_trajectory_planner_B.kstr];
12088 cartesian_trajectory_planner_B.X_f[cartesian_trajectory_planner_B.kstr
12089 + 6 * (cartesian_trajectory_planner_B.b_kstr_lu + 3)] = 0.0;
12090 }
12091 }
12092
12093 for (cartesian_trajectory_planner_B.b_kstr_lu = 0;
12094 cartesian_trajectory_planner_B.b_kstr_lu < 3;
12095 cartesian_trajectory_planner_B.b_kstr_lu++) {
12096 cartesian_trajectory_planner_B.X_f[6 *
12097 cartesian_trajectory_planner_B.b_kstr_lu + 3] =
12098 cartesian_trajectory_planner_B.R_l[3 *
12099 cartesian_trajectory_planner_B.b_kstr_lu];
12100 cartesian_trajectory_planner_B.kstr =
12101 cartesian_trajectory_planner_B.b_kstr_lu << 2;
12102 cartesian_trajectory_planner_B.loop_ub_i = 6 *
12103 (cartesian_trajectory_planner_B.b_kstr_lu + 3);
12104 cartesian_trajectory_planner_B.X_f[cartesian_trajectory_planner_B.loop_ub_i
12105 + 3] =
12106 cartesian_trajectory_planner_B.T[cartesian_trajectory_planner_B.kstr];
12107 cartesian_trajectory_planner_B.X_f[6 *
12108 cartesian_trajectory_planner_B.b_kstr_lu + 4] =
12109 cartesian_trajectory_planner_B.R_l[3 *
12110 cartesian_trajectory_planner_B.b_kstr_lu + 1];
12111 cartesian_trajectory_planner_B.X_f[cartesian_trajectory_planner_B.loop_ub_i
12112 + 4] =
12113 cartesian_trajectory_planner_B.T[cartesian_trajectory_planner_B.kstr +
12114 1];
12115 cartesian_trajectory_planner_B.X_f[6 *
12116 cartesian_trajectory_planner_B.b_kstr_lu + 5] =
12117 cartesian_trajectory_planner_B.R_l[3 *
12118 cartesian_trajectory_planner_B.b_kstr_lu + 2];
12119 cartesian_trajectory_planner_B.X_f[cartesian_trajectory_planner_B.loop_ub_i
12120 + 5] =
12121 cartesian_trajectory_planner_B.T[cartesian_trajectory_planner_B.kstr +
12122 2];
12123 }
12124
12125 cartesian_trajectory_planner_B.b_kstr_lu = b->size[0] * b->size[1];
12126 b->size[0] = 6;
12127 b->size[1] = body->JointInternal.MotionSubspace->size[1];
12128 cartes_emxEnsureCapacity_real_T(b,
12129 cartesian_trajectory_planner_B.b_kstr_lu);
12130 cartesian_trajectory_planner_B.loop_ub_i =
12131 body->JointInternal.MotionSubspace->size[0] *
12132 body->JointInternal.MotionSubspace->size[1] - 1;
12133 for (cartesian_trajectory_planner_B.b_kstr_lu = 0;
12134 cartesian_trajectory_planner_B.b_kstr_lu <=
12135 cartesian_trajectory_planner_B.loop_ub_i;
12136 cartesian_trajectory_planner_B.b_kstr_lu++) {
12137 b->data[cartesian_trajectory_planner_B.b_kstr_lu] =
12138 body->JointInternal.MotionSubspace->
12139 data[cartesian_trajectory_planner_B.b_kstr_lu];
12140 }
12141
12142 cartesian_trajectory_planner_B.n_j = b->size[1] - 1;
12143 cartesian_trajectory_planner_B.b_kstr_lu = JacSlice->size[0] *
12144 JacSlice->size[1];
12145 JacSlice->size[0] = 6;
12146 JacSlice->size[1] = b->size[1];
12147 cartes_emxEnsureCapacity_real_T(JacSlice,
12148 cartesian_trajectory_planner_B.b_kstr_lu);
12149 for (cartesian_trajectory_planner_B.b_kstr_lu = 0;
12150 cartesian_trajectory_planner_B.b_kstr_lu <=
12151 cartesian_trajectory_planner_B.n_j;
12152 cartesian_trajectory_planner_B.b_kstr_lu++) {
12153 cartesian_trajectory_planner_B.coffset_tmp_m =
12154 cartesian_trajectory_planner_B.b_kstr_lu * 6 - 1;
12155 for (cartesian_trajectory_planner_B.kstr = 0;
12156 cartesian_trajectory_planner_B.kstr < 6;
12157 cartesian_trajectory_planner_B.kstr++) {
12158 cartesian_trajectory_planner_B.s_d = 0.0;
12159 for (cartesian_trajectory_planner_B.loop_ub_i = 0;
12160 cartesian_trajectory_planner_B.loop_ub_i < 6;
12161 cartesian_trajectory_planner_B.loop_ub_i++) {
12162 cartesian_trajectory_planner_B.s_d +=
12163 cartesian_trajectory_planner_B.X_f[cartesian_trajectory_planner_B.loop_ub_i
12164 * 6 + cartesian_trajectory_planner_B.kstr] * b->data
12165 [(cartesian_trajectory_planner_B.coffset_tmp_m +
12166 cartesian_trajectory_planner_B.loop_ub_i) + 1];
12167 }
12168
12169 JacSlice->data[(cartesian_trajectory_planner_B.coffset_tmp_m +
12170 cartesian_trajectory_planner_B.kstr) + 1] =
12171 cartesian_trajectory_planner_B.s_d;
12172 }
12173 }
12174
12175 if (cartesian_trajectory_planner_B.endeffectorIndex >
12176 cartesian_trajectory_planner_B.idx_idx_1) {
12177 cartesian_trajectory_planner_B.n_j = 0;
12178 } else {
12179 cartesian_trajectory_planner_B.n_j = static_cast<int32_T>
12180 (cartesian_trajectory_planner_B.endeffectorIndex) - 1;
12181 }
12182
12183 cartesian_trajectory_planner_B.loop_ub_i = JacSlice->size[1];
12184 for (cartesian_trajectory_planner_B.b_kstr_lu = 0;
12185 cartesian_trajectory_planner_B.b_kstr_lu <
12186 cartesian_trajectory_planner_B.loop_ub_i;
12187 cartesian_trajectory_planner_B.b_kstr_lu++) {
12188 for (cartesian_trajectory_planner_B.kstr = 0;
12189 cartesian_trajectory_planner_B.kstr < 6;
12190 cartesian_trajectory_planner_B.kstr++) {
12191 Jac->data[cartesian_trajectory_planner_B.kstr + 6 *
12192 (cartesian_trajectory_planner_B.n_j +
12193 cartesian_trajectory_planner_B.b_kstr_lu)] = JacSlice->data[6 *
12194 cartesian_trajectory_planner_B.b_kstr_lu +
12195 cartesian_trajectory_planner_B.kstr];
12196 }
12197 }
12198 }
12199 }
12200
12201 cartesian_trajec_emxFree_char_T(&bname);
12202 cartesian_trajec_emxFree_real_T(&JacSlice);
12203 cartesian_t_emxFree_f_cell_wrap(&Ttree);
12204 for (cartesian_trajectory_planner_B.b_kstr_lu = 0;
12205 cartesian_trajectory_planner_B.b_kstr_lu < 3;
12206 cartesian_trajectory_planner_B.b_kstr_lu++) {
12207 cartesian_trajectory_planner_B.b_i_a =
12208 cartesian_trajectory_planner_B.b_kstr_lu << 2;
12209 cartesian_trajectory_planner_B.X_f[6 *
12210 cartesian_trajectory_planner_B.b_kstr_lu] =
12211 cartesian_trajectory_planner_B.T2[cartesian_trajectory_planner_B.b_i_a];
12212 cartesian_trajectory_planner_B.kstr = 6 *
12213 (cartesian_trajectory_planner_B.b_kstr_lu + 3);
12214 cartesian_trajectory_planner_B.X_f[cartesian_trajectory_planner_B.kstr] =
12215 0.0;
12216 cartesian_trajectory_planner_B.X_f[6 *
12217 cartesian_trajectory_planner_B.b_kstr_lu + 3] = 0.0;
12218 cartesian_trajectory_planner_B.X_f[cartesian_trajectory_planner_B.kstr + 3] =
12219 cartesian_trajectory_planner_B.T2[cartesian_trajectory_planner_B.b_i_a];
12220 cartesian_trajectory_planner_B.endeffectorIndex =
12221 cartesian_trajectory_planner_B.T2[cartesian_trajectory_planner_B.b_i_a + 1];
12222 cartesian_trajectory_planner_B.X_f[6 *
12223 cartesian_trajectory_planner_B.b_kstr_lu + 1] =
12224 cartesian_trajectory_planner_B.endeffectorIndex;
12225 cartesian_trajectory_planner_B.X_f[cartesian_trajectory_planner_B.kstr + 1] =
12226 0.0;
12227 cartesian_trajectory_planner_B.X_f[6 *
12228 cartesian_trajectory_planner_B.b_kstr_lu + 4] = 0.0;
12229 cartesian_trajectory_planner_B.X_f[cartesian_trajectory_planner_B.kstr + 4] =
12230 cartesian_trajectory_planner_B.endeffectorIndex;
12231 cartesian_trajectory_planner_B.endeffectorIndex =
12232 cartesian_trajectory_planner_B.T2[cartesian_trajectory_planner_B.b_i_a + 2];
12233 cartesian_trajectory_planner_B.X_f[6 *
12234 cartesian_trajectory_planner_B.b_kstr_lu + 2] =
12235 cartesian_trajectory_planner_B.endeffectorIndex;
12236 cartesian_trajectory_planner_B.X_f[cartesian_trajectory_planner_B.kstr + 2] =
12237 0.0;
12238 cartesian_trajectory_planner_B.X_f[6 *
12239 cartesian_trajectory_planner_B.b_kstr_lu + 5] = 0.0;
12240 cartesian_trajectory_planner_B.X_f[cartesian_trajectory_planner_B.kstr + 5] =
12241 cartesian_trajectory_planner_B.endeffectorIndex;
12242 }
12243
12244 cartesian_trajectory_planner_B.n_j = Jac->size[1];
12245 cartesian_trajectory_planner_B.b_kstr_lu = b->size[0] * b->size[1];
12246 b->size[0] = 6;
12247 b->size[1] = Jac->size[1];
12248 cartes_emxEnsureCapacity_real_T(b, cartesian_trajectory_planner_B.b_kstr_lu);
12249 cartesian_trajectory_planner_B.loop_ub_i = Jac->size[0] * Jac->size[1] - 1;
12250 for (cartesian_trajectory_planner_B.b_kstr_lu = 0;
12251 cartesian_trajectory_planner_B.b_kstr_lu <=
12252 cartesian_trajectory_planner_B.loop_ub_i;
12253 cartesian_trajectory_planner_B.b_kstr_lu++) {
12254 b->data[cartesian_trajectory_planner_B.b_kstr_lu] = Jac->
12255 data[cartesian_trajectory_planner_B.b_kstr_lu];
12256 }
12257
12258 cartesian_trajectory_planner_B.b_kstr_lu = Jac->size[0] * Jac->size[1];
12259 Jac->size[0] = 6;
12260 Jac->size[1] = cartesian_trajectory_planner_B.n_j;
12261 cartes_emxEnsureCapacity_real_T(Jac, cartesian_trajectory_planner_B.b_kstr_lu);
12262 for (cartesian_trajectory_planner_B.b_kstr_lu = 0;
12263 cartesian_trajectory_planner_B.b_kstr_lu <
12264 cartesian_trajectory_planner_B.n_j;
12265 cartesian_trajectory_planner_B.b_kstr_lu++) {
12266 cartesian_trajectory_planner_B.coffset_tmp_m =
12267 cartesian_trajectory_planner_B.b_kstr_lu * 6 - 1;
12268 for (cartesian_trajectory_planner_B.b_i_a = 0;
12269 cartesian_trajectory_planner_B.b_i_a < 6;
12270 cartesian_trajectory_planner_B.b_i_a++) {
12271 cartesian_trajectory_planner_B.s_d = 0.0;
12272 for (cartesian_trajectory_planner_B.loop_ub_i = 0;
12273 cartesian_trajectory_planner_B.loop_ub_i < 6;
12274 cartesian_trajectory_planner_B.loop_ub_i++) {
12275 cartesian_trajectory_planner_B.s_d +=
12276 cartesian_trajectory_planner_B.X_f[cartesian_trajectory_planner_B.loop_ub_i
12277 * 6 + cartesian_trajectory_planner_B.b_i_a] * b->data
12278 [(cartesian_trajectory_planner_B.coffset_tmp_m +
12279 cartesian_trajectory_planner_B.loop_ub_i) + 1];
12280 }
12281
12282 Jac->data[(cartesian_trajectory_planner_B.coffset_tmp_m +
12283 cartesian_trajectory_planner_B.b_i_a) + 1] =
12284 cartesian_trajectory_planner_B.s_d;
12285 }
12286 }
12287
12288 cartesian_trajec_emxFree_real_T(&b);
12289}
12290
12291void rt_invd6x6_snf(const real_T u[36], real_T y[36])
12292{
12293 int8_T p[6];
12294 real_T A[36];
12295 int8_T ipiv[6];
12296 int32_T jj;
12297 int32_T j;
12298 int32_T kAcol;
12299 int32_T c;
12300 int32_T ix;
12301 real_T smax;
12302 real_T s;
12303 int32_T iy;
12304 int32_T jy;
12305 int32_T j_0;
12306 int32_T ijA;
12307 for (iy = 0; iy < 36; iy++) {
12308 y[iy] = 0.0;
12309 A[iy] = u[iy];
12310 }
12311
12312 for (iy = 0; iy < 6; iy++) {
12313 ipiv[iy] = static_cast<int8_T>(iy + 1);
12314 }
12315
12316 for (j = 0; j < 5; j++) {
12317 c = j * 7 + 2;
12318 jj = j * 7;
12319 kAcol = 6 - j;
12320 iy = 1;
12321 ix = c - 2;
12322 smax = fabs(A[jj]);
12323 for (jy = 2; jy <= kAcol; jy++) {
12324 ix++;
12325 s = fabs(A[ix]);
12326 if (s > smax) {
12327 iy = jy;
12328 smax = s;
12329 }
12330 }
12331
12332 if (A[(c + iy) - 3] != 0.0) {
12333 if (iy - 1 != 0) {
12334 jy = j + iy;
12335 ipiv[j] = static_cast<int8_T>(jy);
12336 ix = j;
12337 iy = jy - 1;
12338 for (jy = 0; jy < 6; jy++) {
12339 smax = A[ix];
12340 A[ix] = A[iy];
12341 A[iy] = smax;
12342 ix += 6;
12343 iy += 6;
12344 }
12345 }
12346
12347 iy = c - j;
12348 for (ix = c; ix <= iy + 4; ix++) {
12349 A[ix - 1] /= A[jj];
12350 }
12351 }
12352
12353 kAcol = 4 - j;
12354 jy = jj + 6;
12355 for (j_0 = 0; j_0 <= kAcol; j_0++) {
12356 if (A[jy] != 0.0) {
12357 smax = -A[jy];
12358 ix = c - 1;
12359 iy = jj - j;
12360 for (ijA = jj + 8; ijA <= iy + 12; ijA++) {
12361 A[ijA - 1] += A[ix] * smax;
12362 ix++;
12363 }
12364 }
12365
12366 jy += 6;
12367 jj += 6;
12368 }
12369 }
12370
12371 for (iy = 0; iy < 6; iy++) {
12372 p[iy] = static_cast<int8_T>(iy + 1);
12373 }
12374
12375 for (jy = 0; jy < 5; jy++) {
12376 if (ipiv[jy] > jy + 1) {
12377 j = ipiv[jy] - 1;
12378 iy = p[j];
12379 p[j] = p[jy];
12380 p[jy] = static_cast<int8_T>(iy);
12381 }
12382 }
12383
12384 for (jy = 0; jy < 6; jy++) {
12385 jj = p[jy] - 1;
12386 y[jy + 6 * jj] = 1.0;
12387 for (j = jy + 1; j < 7; j++) {
12388 iy = (6 * jj + j) - 1;
12389 if (y[iy] != 0.0) {
12390 for (ix = j + 1; ix < 7; ix++) {
12391 c = (6 * jj + ix) - 1;
12392 y[c] -= A[((j - 1) * 6 + ix) - 1] * y[iy];
12393 }
12394 }
12395 }
12396 }
12397
12398 for (j = 0; j < 6; j++) {
12399 jj = 6 * j;
12400 for (jy = 5; jy >= 0; jy--) {
12401 kAcol = 6 * jy;
12402 iy = jy + jj;
12403 if (y[iy] != 0.0) {
12404 y[iy] /= A[jy + kAcol];
12405 j_0 = jy - 1;
12406 for (ix = 0; ix <= j_0; ix++) {
12407 c = ix + jj;
12408 y[c] -= y[iy] * A[ix + kAcol];
12409 }
12410 }
12411 }
12412 }
12413}
12414
12415static void matlabCodegenHandle_matlabC_ast(ros_slros_internal_block_Subs_T *obj)
12416{
12417 if (!obj->matlabCodegenIsDeleted) {
12418 obj->matlabCodegenIsDeleted = true;
12419 }
12420}
12421
12422static void matlabCodegenHandle_matlab_astw(ros_slros_internal_block_GetP_T *obj)
12423{
12424 if (!obj->matlabCodegenIsDeleted) {
12425 obj->matlabCodegenIsDeleted = true;
12426 }
12427}
12428
12429static void matlabCodegenHandle_matlabCod_a(robotics_slmanip_internal_b_a_T *obj)
12430{
12431 if (!obj->matlabCodegenIsDeleted) {
12432 obj->matlabCodegenIsDeleted = true;
12433 }
12434}
12435
12436static void cartesian_tr_SystemCore_release(b_inverseKinematics_cartesian_T *obj)
12437{
12438 if (obj->isInitialized == 1) {
12439 obj->isInitialized = 2;
12440 }
12441}
12442
12443static void cartesian_tra_SystemCore_delete(b_inverseKinematics_cartesian_T *obj)
12444{
12445 cartesian_tr_SystemCore_release(obj);
12446}
12447
12448static void matlabCodegenHandle_matlabCodeg(b_inverseKinematics_cartesian_T *obj)
12449{
12450 if (!obj->matlabCodegenIsDeleted) {
12451 obj->matlabCodegenIsDeleted = true;
12452 cartesian_tra_SystemCore_delete(obj);
12453 }
12454}
12455
12456static void emxFreeStruct_c_rigidBodyJoint(c_rigidBodyJoint_cartesian__a_T
12457 *pStruct)
12458{
12459 cartesian_trajec_emxFree_char_T(&pStruct->Type);
12460 cartesian_trajec_emxFree_real_T(&pStruct->MotionSubspace);
12461 cartesian_trajec_emxFree_char_T(&pStruct->NameInternal);
12462 cartesian_trajec_emxFree_real_T(&pStruct->PositionLimitsInternal);
12463 cartesian_trajec_emxFree_real_T(&pStruct->HomePositionInternal);
12464}
12465
12466static void emxFreeStruct_v_robotics_manip_(v_robotics_manip_internal_Rig_T
12467 *pStruct)
12468{
12469 cartesian_trajec_emxFree_char_T(&pStruct->NameInternal);
12470 emxFreeStruct_c_rigidBodyJoint(&pStruct->JointInternal);
12471}
12472
12473static void emxFreeStruct_y_robotics_manip_(y_robotics_manip_internal_Rig_T
12474 *pStruct)
12475{
12476 emxFreeStruct_v_robotics_manip_(&pStruct->Base);
12477}
12478
12479static void emxFreeStruct_b_inverseKinemati(b_inverseKinematics_cartesian_T
12480 *pStruct)
12481{
12482 cartesian_trajec_emxFree_real_T(&pStruct->Limits);
12483}
12484
12485static void emxFreeStruct_robotics_slmanip_(robotics_slmanip_internal_b_a_T
12486 *pStruct)
12487{
12488 emxFreeStruct_y_robotics_manip_(&pStruct->TreeInternal);
12489 emxFreeStruct_b_inverseKinemati(&pStruct->IKInternal);
12490}
12491
12492static void emxFreeStruct_w_robotics_manip_(w_robotics_manip_internal_Rig_T
12493 *pStruct)
12494{
12495 cartesian_trajec_emxFree_char_T(&pStruct->NameInternal);
12496}
12497
12498static void emxFreeStruct_x_robotics_manip_(x_robotics_manip_internal_Rig_T
12499 *pStruct)
12500{
12501 emxFreeStruct_w_robotics_manip_(&pStruct->Base);
12502}
12503
12504static void emxFreeStruct_f_robotics_manip_(f_robotics_manip_internal_IKE_T
12505 *pStruct)
12506{
12507 cartesian_trajec_emxFree_char_T(&pStruct->BodyName);
12508 cartesian_trajec_emxFree_real_T(&pStruct->ErrTemp);
12509 cartesian_trajec_emxFree_real_T(&pStruct->GradTemp);
12510}
12511
12512static void emxFreeStruct_h_robotics_core_i(h_robotics_core_internal_Damp_T
12513 *pStruct)
12514{
12515 cartesian_trajec_emxFree_real_T(&pStruct->ConstraintMatrix);
12516 cartesian_trajec_emxFree_real_T(&pStruct->ConstraintBound);
12517}
12518
12519static void emxFreeStruct_c_rigidBodyJoint1(c_rigidBodyJoint_cartesian_tr_T
12520 *pStruct)
12521{
12522 cartesian_trajec_emxFree_char_T(&pStruct->Type);
12523 cartesian_trajec_emxFree_real_T(&pStruct->MotionSubspace);
12524}
12525
12526static void emxFreeStruct_o_robotics_manip_(o_robotics_manip_internal_Rig_T
12527 *pStruct)
12528{
12529 cartesian_trajec_emxFree_char_T(&pStruct->NameInternal);
12530 emxFreeStruct_c_rigidBodyJoint1(&pStruct->JointInternal);
12531}
12532
12533static void emxFreeStruct_p_robotics_manip_(p_robotics_manip_internal_Rig_T
12534 *pStruct)
12535{
12536 emxFreeStruct_o_robotics_manip_(&pStruct->Base);
12537}
12538
12539static void emxFreeStruct_robotics_slmani_a(robotics_slmanip_internal_blo_T
12540 *pStruct)
12541{
12542 emxFreeStruct_p_robotics_manip_(&pStruct->TreeInternal);
12543}
12544
12545static void emxFreeStruct_n_robotics_manip_(n_robotics_manip_internal_Rig_T
12546 *pStruct)
12547{
12548 cartesian_trajec_emxFree_char_T(&pStruct->NameInternal);
12549 emxFreeStruct_c_rigidBodyJoint1(&pStruct->JointInternal);
12550}
12551
12552static void matlabCodegenHandle_matlabCo_as(ros_slros_internal_block_Publ_T *obj)
12553{
12554 if (!obj->matlabCodegenIsDeleted) {
12555 obj->matlabCodegenIsDeleted = true;
12556 }
12557}
12558
12559static void cartesian_traj_SystemCore_setup(robotics_slcore_internal_bl_a_T *obj)
12560{
12561 obj->isInitialized = 1;
12562 obj->TunablePropsChanged = false;
12563}
12564
12565static void emxInitStruct_c_rigidBodyJoint(c_rigidBodyJoint_cartesian__a_T
12566 *pStruct)
12567{
12568 cartesian_trajec_emxInit_char_T(&pStruct->Type, 2);
12569 cartesian_trajec_emxInit_real_T(&pStruct->MotionSubspace, 2);
12570 cartesian_trajec_emxInit_char_T(&pStruct->NameInternal, 2);
12571 cartesian_trajec_emxInit_real_T(&pStruct->PositionLimitsInternal, 2);
12572 cartesian_trajec_emxInit_real_T(&pStruct->HomePositionInternal, 1);
12573}
12574
12575static void emxInitStruct_v_robotics_manip_(v_robotics_manip_internal_Rig_T
12576 *pStruct)
12577{
12578 cartesian_trajec_emxInit_char_T(&pStruct->NameInternal, 2);
12579 emxInitStruct_c_rigidBodyJoint(&pStruct->JointInternal);
12580}
12581
12582static void emxInitStruct_y_robotics_manip_(y_robotics_manip_internal_Rig_T
12583 *pStruct)
12584{
12585 emxInitStruct_v_robotics_manip_(&pStruct->Base);
12586}
12587
12588static void emxInitStruct_b_inverseKinemati(b_inverseKinematics_cartesian_T
12589 *pStruct)
12590{
12591 cartesian_trajec_emxInit_real_T(&pStruct->Limits, 2);
12592}
12593
12594static void emxInitStruct_robotics_slmanip_(robotics_slmanip_internal_b_a_T
12595 *pStruct)
12596{
12597 emxInitStruct_y_robotics_manip_(&pStruct->TreeInternal);
12598 emxInitStruct_b_inverseKinemati(&pStruct->IKInternal);
12599}
12600
12601static void emxInitStruct_w_robotics_manip_(w_robotics_manip_internal_Rig_T
12602 *pStruct)
12603{
12604 cartesian_trajec_emxInit_char_T(&pStruct->NameInternal, 2);
12605}
12606
12607static void emxInitStruct_x_robotics_manip_(x_robotics_manip_internal_Rig_T
12608 *pStruct)
12609{
12610 emxInitStruct_w_robotics_manip_(&pStruct->Base);
12611}
12612
12613static void emxInitStruct_f_robotics_manip_(f_robotics_manip_internal_IKE_T
12614 *pStruct)
12615{
12616 cartesian_trajec_emxInit_char_T(&pStruct->BodyName, 2);
12617 cartesian_trajec_emxInit_real_T(&pStruct->ErrTemp, 1);
12618 cartesian_trajec_emxInit_real_T(&pStruct->GradTemp, 1);
12619}
12620
12621static void emxInitStruct_h_robotics_core_i(h_robotics_core_internal_Damp_T
12622 *pStruct)
12623{
12624 cartesian_trajec_emxInit_real_T(&pStruct->ConstraintMatrix, 2);
12625 cartesian_trajec_emxInit_real_T(&pStruct->ConstraintBound, 1);
12626}
12627
12628static void cartesia_twister_state_vector_a(uint32_T mt[625])
12629{
12630 uint32_T r;
12631 int32_T b_mti;
12632 r = 5489U;
12633 mt[0] = 5489U;
12634 for (b_mti = 0; b_mti < 623; b_mti++) {
12635 r = ((r >> 30U ^ r) * 1812433253U + b_mti) + 1U;
12636 mt[b_mti + 1] = r;
12637 }
12638
12639 mt[624] = 624U;
12640}
12641
12642static void cartesian_tr_eml_rand_mt19937ar(uint32_T state[625])
12643{
12644 memset(&state[0], 0, 625U * sizeof(uint32_T));
12645 cartesia_twister_state_vector_a(state);
12646}
12647
12648static v_robotics_manip_internal_Rig_T *c_RigidBody_RigidBody_astwhqf2a
12649 (v_robotics_manip_internal_Rig_T *obj)
12650{
12651 v_robotics_manip_internal_Rig_T *b_obj;
12652 int8_T msubspace_data[36];
12653 real_T poslim_data[12];
12654 emxArray_char_T_cartesian_tra_T *switch_expression;
12655 boolean_T b_bool;
12656 int32_T b_kstr;
12657 char_T b[8];
12658 char_T b_0[9];
12659 int32_T loop_ub;
12660 int8_T tmp[6];
12661 static const char_T tmp_0[13] = { 'e', 'd', 'o', '_', 'b', 'a', 's', 'e', '_',
12662 'l', 'i', 'n', 'k' };
12663
12664 static const real_T tmp_1[9] = { 0.012583419040406959, -0.00021487638648447484,
12665 -0.00022605919127205462, -0.00021487638648447484, 0.00052369449451288713,
12666 -0.00011525315957400814, -0.00022605919127205462, -0.00011525315957400814,
12667 0.012646079447789898 };
12668
12669 static const real_T tmp_2[36] = { 0.012583419040406959,
12670 -0.00021487638648447484, -0.00022605919127205462, 0.0, -0.00392971169381184,
12671 0.00047022930128152475, -0.00021487638648447484, 0.00052369449451288713,
12672 -0.00011525315957400814, 0.00392971169381184, 0.0, -0.00449464704691423,
12673 -0.00022605919127205462, -0.00011525315957400814, 0.012646079447789898,
12674 -0.00047022930128152475, 0.00449464704691423, 0.0, 0.0, 0.00392971169381184,
12675 -0.00047022930128152475, 0.0785942338762368, 0.0, 0.0, -0.00392971169381184,
12676 0.0, 0.00449464704691423, 0.0, 0.0785942338762368, 0.0,
12677 0.00047022930128152475, -0.00449464704691423, 0.0, 0.0, 0.0,
12678 0.0785942338762368 };
12679
12680 static const int8_T tmp_3[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,
12681 1 };
12682
12683 static const char_T tmp_4[15] = { 'w', 'o', 'r', 'l', 'd', '_', 'e', 'd', 'o',
12684 '_', 'j', 'o', 'i', 'n', 't' };
12685
12686 static const char_T tmp_5[5] = { 'f', 'i', 'x', 'e', 'd' };
12687
12688 static const char_T tmp_6[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
12689
12690 static const char_T tmp_7[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
12691
12692 static const real_T tmp_8[16] = { 1.0, 0.0, -0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
12693 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
12694
12695 static const real_T tmp_9[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
12696 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
12697
12698 int32_T exitg1;
12699 b_obj = obj;
12700 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
12701 obj->NameInternal->size[0] = 1;
12702 obj->NameInternal->size[1] = 13;
12703 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);
12704 for (b_kstr = 0; b_kstr < 13; b_kstr++) {
12705 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
12706 }
12707
12708 obj->ParentIndex = 0.0;
12709 obj->MassInternal = 0.0785942338762368;
12710 obj->CenterOfMassInternal[0] = 0.057188;
12711 obj->CenterOfMassInternal[1] = 0.005983;
12712 obj->CenterOfMassInternal[2] = 0.05;
12713 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
12714 obj->InertiaInternal[b_kstr] = tmp_1[b_kstr];
12715 }
12716
12717 for (b_kstr = 0; b_kstr < 36; b_kstr++) {
12718 obj->SpatialInertia[b_kstr] = tmp_2[b_kstr];
12719 }
12720
12721 obj->JointInternal.InTree = false;
12722 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
12723 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_3[b_kstr];
12724 }
12725
12726 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
12727 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_3[b_kstr];
12728 }
12729
12730 b_kstr = obj->JointInternal.NameInternal->size[0] *
12731 obj->JointInternal.NameInternal->size[1];
12732 obj->JointInternal.NameInternal->size[0] = 1;
12733 obj->JointInternal.NameInternal->size[1] = 15;
12734 cartes_emxEnsureCapacity_char_T(obj->JointInternal.NameInternal, b_kstr);
12735 for (b_kstr = 0; b_kstr < 15; b_kstr++) {
12736 obj->JointInternal.NameInternal->data[b_kstr] = tmp_4[b_kstr];
12737 }
12738
12739 b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];
12740 obj->JointInternal.Type->size[0] = 1;
12741 obj->JointInternal.Type->size[1] = 5;
12742 cartes_emxEnsureCapacity_char_T(obj->JointInternal.Type, b_kstr);
12743 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
12744 obj->JointInternal.Type->data[b_kstr] = tmp_5[b_kstr];
12745 }
12746
12747 cartesian_trajec_emxInit_char_T(&switch_expression, 2);
12748 b_kstr = switch_expression->size[0] * switch_expression->size[1];
12749 switch_expression->size[0] = 1;
12750 switch_expression->size[1] = obj->JointInternal.Type->size[1];
12751 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
12752 loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]
12753 - 1;
12754 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
12755 switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];
12756 }
12757
12758 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
12759 b[b_kstr] = tmp_6[b_kstr];
12760 }
12761
12762 b_bool = false;
12763 if (switch_expression->size[1] == 8) {
12764 b_kstr = 1;
12765 do {
12766 exitg1 = 0;
12767 if (b_kstr - 1 < 8) {
12768 loop_ub = b_kstr - 1;
12769 if (switch_expression->data[loop_ub] != b[loop_ub]) {
12770 exitg1 = 1;
12771 } else {
12772 b_kstr++;
12773 }
12774 } else {
12775 b_bool = true;
12776 exitg1 = 1;
12777 }
12778 } while (exitg1 == 0);
12779 }
12780
12781 if (b_bool) {
12782 b_kstr = 0;
12783 } else {
12784 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
12785 b_0[b_kstr] = tmp_7[b_kstr];
12786 }
12787
12788 b_bool = false;
12789 if (switch_expression->size[1] == 9) {
12790 b_kstr = 1;
12791 do {
12792 exitg1 = 0;
12793 if (b_kstr - 1 < 9) {
12794 loop_ub = b_kstr - 1;
12795 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
12796 exitg1 = 1;
12797 } else {
12798 b_kstr++;
12799 }
12800 } else {
12801 b_bool = true;
12802 exitg1 = 1;
12803 }
12804 } while (exitg1 == 0);
12805 }
12806
12807 if (b_bool) {
12808 b_kstr = 1;
12809 } else {
12810 b_kstr = -1;
12811 }
12812 }
12813
12814 cartesian_trajec_emxFree_char_T(&switch_expression);
12815 switch (b_kstr) {
12816 case 0:
12817 tmp[0] = 0;
12818 tmp[1] = 0;
12819 tmp[2] = 1;
12820 tmp[3] = 0;
12821 tmp[4] = 0;
12822 tmp[5] = 0;
12823 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
12824 msubspace_data[b_kstr] = tmp[b_kstr];
12825 }
12826
12827 poslim_data[0] = -3.1415926535897931;
12828 poslim_data[1] = 3.1415926535897931;
12829 obj->JointInternal.VelocityNumber = 1.0;
12830 obj->JointInternal.PositionNumber = 1.0;
12831 obj->JointInternal.JointAxisInternal[0] = 0.0;
12832 obj->JointInternal.JointAxisInternal[1] = 0.0;
12833 obj->JointInternal.JointAxisInternal[2] = 1.0;
12834 break;
12835
12836 case 1:
12837 tmp[0] = 0;
12838 tmp[1] = 0;
12839 tmp[2] = 0;
12840 tmp[3] = 0;
12841 tmp[4] = 0;
12842 tmp[5] = 1;
12843 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
12844 msubspace_data[b_kstr] = tmp[b_kstr];
12845 }
12846
12847 poslim_data[0] = -0.5;
12848 poslim_data[1] = 0.5;
12849 obj->JointInternal.VelocityNumber = 1.0;
12850 obj->JointInternal.PositionNumber = 1.0;
12851 obj->JointInternal.JointAxisInternal[0] = 0.0;
12852 obj->JointInternal.JointAxisInternal[1] = 0.0;
12853 obj->JointInternal.JointAxisInternal[2] = 1.0;
12854 break;
12855
12856 default:
12857 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
12858 msubspace_data[b_kstr] = 0;
12859 }
12860
12861 poslim_data[0] = 0.0;
12862 poslim_data[1] = 0.0;
12863 obj->JointInternal.VelocityNumber = 0.0;
12864 obj->JointInternal.PositionNumber = 0.0;
12865 obj->JointInternal.JointAxisInternal[0] = 0.0;
12866 obj->JointInternal.JointAxisInternal[1] = 0.0;
12867 obj->JointInternal.JointAxisInternal[2] = 0.0;
12868 break;
12869 }
12870
12871 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
12872 obj->JointInternal.MotionSubspace->size[1];
12873 obj->JointInternal.MotionSubspace->size[0] = 6;
12874 obj->JointInternal.MotionSubspace->size[1] = 1;
12875 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
12876 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
12877 obj->JointInternal.MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
12878 }
12879
12880 b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *
12881 obj->JointInternal.PositionLimitsInternal->size[1];
12882 obj->JointInternal.PositionLimitsInternal->size[0] = 1;
12883 obj->JointInternal.PositionLimitsInternal->size[1] = 2;
12884 cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,
12885 b_kstr);
12886 for (b_kstr = 0; b_kstr < 2; b_kstr++) {
12887 obj->JointInternal.PositionLimitsInternal->data[b_kstr] = poslim_data[b_kstr];
12888 }
12889
12890 b_kstr = obj->JointInternal.HomePositionInternal->size[0];
12891 obj->JointInternal.HomePositionInternal->size[0] = 1;
12892 cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,
12893 b_kstr);
12894 for (b_kstr = 0; b_kstr < 1; b_kstr++) {
12895 obj->JointInternal.HomePositionInternal->data[0] = 0.0;
12896 }
12897
12898 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
12899 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_8[b_kstr];
12900 }
12901
12902 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
12903 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_9[b_kstr];
12904 }
12905
12906 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
12907 obj->JointInternal.MotionSubspace->size[1];
12908 obj->JointInternal.MotionSubspace->size[0] = 6;
12909 obj->JointInternal.MotionSubspace->size[1] = 1;
12910 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
12911 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
12912 obj->JointInternal.MotionSubspace->data[b_kstr] = 0.0;
12913 }
12914
12915 obj->JointInternal.InTree = true;
12916 b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *
12917 obj->JointInternal.PositionLimitsInternal->size[1];
12918 obj->JointInternal.PositionLimitsInternal->size[0] = 1;
12919 obj->JointInternal.PositionLimitsInternal->size[1] = 2;
12920 cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,
12921 b_kstr);
12922 obj->JointInternal.PositionLimitsInternal->data[0] = 0.0;
12923 obj->JointInternal.PositionLimitsInternal->data
12924 [obj->JointInternal.PositionLimitsInternal->size[0]] = 0.0;
12925 obj->JointInternal.JointAxisInternal[0] = 0.0;
12926 obj->JointInternal.JointAxisInternal[1] = 0.0;
12927 obj->JointInternal.JointAxisInternal[2] = 0.0;
12928 b_kstr = obj->JointInternal.HomePositionInternal->size[0];
12929 obj->JointInternal.HomePositionInternal->size[0] = 1;
12930 cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,
12931 b_kstr);
12932 obj->JointInternal.HomePositionInternal->data[0] = 0.0;
12933 return b_obj;
12934}
12935
12936static v_robotics_manip_internal_Rig_T *RigidBody_RigidBody_astwhqf2az
12937 (v_robotics_manip_internal_Rig_T *obj)
12938{
12939 v_robotics_manip_internal_Rig_T *b_obj;
12940 int8_T msubspace_data[36];
12941 real_T poslim_data[12];
12942 emxArray_char_T_cartesian_tra_T *switch_expression;
12943 boolean_T b_bool;
12944 int32_T b_kstr;
12945 char_T b[8];
12946 char_T b_0[9];
12947 int32_T loop_ub;
12948 int8_T tmp[6];
12949 static const char_T tmp_0[10] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
12950 '1' };
12951
12952 static const real_T tmp_1[9] = { 0.012559660892485551, 0.00032713982710414,
12953 -1.32683892634271E-6, 0.00032713982710414, 0.00018048007331848145,
12954 9.17416945099368E-5, -1.32683892634271E-6, 9.17416945099368E-5,
12955 0.012672078048055372 };
12956
12957 static const real_T tmp_2[36] = { 0.012559660892485551, 0.00032713982710414,
12958 -1.32683892634271E-6, 0.0, -0.0, 0.0037143634929909515, 0.00032713982710414,
12959 0.00018048007331848145, 9.17416945099368E-5, 0.0, 0.0, 0.0029444543779393356,
12960 -1.32683892634271E-6, 9.17416945099368E-5, 0.012672078048055372,
12961 -0.0037143634929909515, -0.0029444543779393356, 0.0, 0.0, 0.0,
12962 -0.0037143634929909515, 0.0785942338762368, 0.0, 0.0, -0.0, 0.0,
12963 -0.0029444543779393356, 0.0, 0.0785942338762368, 0.0, 0.0037143634929909515,
12964 0.0029444543779393356, 0.0, 0.0, 0.0, 0.0785942338762368 };
12965
12966 static const int8_T tmp_3[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,
12967 1 };
12968
12969 static const char_T tmp_4[11] = { 'e', 'd', 'o', '_', 'j', 'o', 'i', 'n', 't',
12970 '_', '1' };
12971
12972 static const char_T tmp_5[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
12973
12974 static const char_T tmp_6[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
12975
12976 static const real_T tmp_7[16] = { 1.0, 0.0, -0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
12977 0.0, 1.0, 0.0, 0.0, 0.0, 0.337, 1.0 };
12978
12979 static const real_T tmp_8[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
12980 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
12981
12982 static const real_T tmp_9[36] = { 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
12983 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
12984 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
12985
12986 int32_T exitg1;
12987 b_obj = obj;
12988 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
12989 obj->NameInternal->size[0] = 1;
12990 obj->NameInternal->size[1] = 10;
12991 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);
12992 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
12993 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
12994 }
12995
12996 obj->ParentIndex = 1.0;
12997 obj->MassInternal = 0.0785942338762368;
12998 obj->CenterOfMassInternal[0] = -0.037464;
12999 obj->CenterOfMassInternal[1] = 0.04726;
13000 obj->CenterOfMassInternal[2] = 0.0;
13001 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
13002 obj->InertiaInternal[b_kstr] = tmp_1[b_kstr];
13003 }
13004
13005 for (b_kstr = 0; b_kstr < 36; b_kstr++) {
13006 obj->SpatialInertia[b_kstr] = tmp_2[b_kstr];
13007 }
13008
13009 obj->JointInternal.InTree = false;
13010 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
13011 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_3[b_kstr];
13012 }
13013
13014 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
13015 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_3[b_kstr];
13016 }
13017
13018 b_kstr = obj->JointInternal.NameInternal->size[0] *
13019 obj->JointInternal.NameInternal->size[1];
13020 obj->JointInternal.NameInternal->size[0] = 1;
13021 obj->JointInternal.NameInternal->size[1] = 11;
13022 cartes_emxEnsureCapacity_char_T(obj->JointInternal.NameInternal, b_kstr);
13023 for (b_kstr = 0; b_kstr < 11; b_kstr++) {
13024 obj->JointInternal.NameInternal->data[b_kstr] = tmp_4[b_kstr];
13025 }
13026
13027 b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];
13028 obj->JointInternal.Type->size[0] = 1;
13029 obj->JointInternal.Type->size[1] = 8;
13030 cartes_emxEnsureCapacity_char_T(obj->JointInternal.Type, b_kstr);
13031 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
13032 obj->JointInternal.Type->data[b_kstr] = tmp_5[b_kstr];
13033 }
13034
13035 cartesian_trajec_emxInit_char_T(&switch_expression, 2);
13036 b_kstr = switch_expression->size[0] * switch_expression->size[1];
13037 switch_expression->size[0] = 1;
13038 switch_expression->size[1] = obj->JointInternal.Type->size[1];
13039 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
13040 loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]
13041 - 1;
13042 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
13043 switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];
13044 }
13045
13046 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
13047 b[b_kstr] = tmp_5[b_kstr];
13048 }
13049
13050 b_bool = false;
13051 if (switch_expression->size[1] == 8) {
13052 b_kstr = 1;
13053 do {
13054 exitg1 = 0;
13055 if (b_kstr - 1 < 8) {
13056 loop_ub = b_kstr - 1;
13057 if (switch_expression->data[loop_ub] != b[loop_ub]) {
13058 exitg1 = 1;
13059 } else {
13060 b_kstr++;
13061 }
13062 } else {
13063 b_bool = true;
13064 exitg1 = 1;
13065 }
13066 } while (exitg1 == 0);
13067 }
13068
13069 if (b_bool) {
13070 b_kstr = 0;
13071 } else {
13072 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
13073 b_0[b_kstr] = tmp_6[b_kstr];
13074 }
13075
13076 b_bool = false;
13077 if (switch_expression->size[1] == 9) {
13078 b_kstr = 1;
13079 do {
13080 exitg1 = 0;
13081 if (b_kstr - 1 < 9) {
13082 loop_ub = b_kstr - 1;
13083 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
13084 exitg1 = 1;
13085 } else {
13086 b_kstr++;
13087 }
13088 } else {
13089 b_bool = true;
13090 exitg1 = 1;
13091 }
13092 } while (exitg1 == 0);
13093 }
13094
13095 if (b_bool) {
13096 b_kstr = 1;
13097 } else {
13098 b_kstr = -1;
13099 }
13100 }
13101
13102 cartesian_trajec_emxFree_char_T(&switch_expression);
13103 switch (b_kstr) {
13104 case 0:
13105 tmp[0] = 0;
13106 tmp[1] = 0;
13107 tmp[2] = 1;
13108 tmp[3] = 0;
13109 tmp[4] = 0;
13110 tmp[5] = 0;
13111 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
13112 msubspace_data[b_kstr] = tmp[b_kstr];
13113 }
13114
13115 poslim_data[0] = -3.1415926535897931;
13116 poslim_data[1] = 3.1415926535897931;
13117 obj->JointInternal.VelocityNumber = 1.0;
13118 obj->JointInternal.PositionNumber = 1.0;
13119 obj->JointInternal.JointAxisInternal[0] = 0.0;
13120 obj->JointInternal.JointAxisInternal[1] = 0.0;
13121 obj->JointInternal.JointAxisInternal[2] = 1.0;
13122 break;
13123
13124 case 1:
13125 tmp[0] = 0;
13126 tmp[1] = 0;
13127 tmp[2] = 0;
13128 tmp[3] = 0;
13129 tmp[4] = 0;
13130 tmp[5] = 1;
13131 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
13132 msubspace_data[b_kstr] = tmp[b_kstr];
13133 }
13134
13135 poslim_data[0] = -0.5;
13136 poslim_data[1] = 0.5;
13137 obj->JointInternal.VelocityNumber = 1.0;
13138 obj->JointInternal.PositionNumber = 1.0;
13139 obj->JointInternal.JointAxisInternal[0] = 0.0;
13140 obj->JointInternal.JointAxisInternal[1] = 0.0;
13141 obj->JointInternal.JointAxisInternal[2] = 1.0;
13142 break;
13143
13144 default:
13145 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
13146 msubspace_data[b_kstr] = 0;
13147 }
13148
13149 poslim_data[0] = 0.0;
13150 poslim_data[1] = 0.0;
13151 obj->JointInternal.VelocityNumber = 0.0;
13152 obj->JointInternal.PositionNumber = 0.0;
13153 obj->JointInternal.JointAxisInternal[0] = 0.0;
13154 obj->JointInternal.JointAxisInternal[1] = 0.0;
13155 obj->JointInternal.JointAxisInternal[2] = 0.0;
13156 break;
13157 }
13158
13159 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
13160 obj->JointInternal.MotionSubspace->size[1];
13161 obj->JointInternal.MotionSubspace->size[0] = 6;
13162 obj->JointInternal.MotionSubspace->size[1] = 1;
13163 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
13164 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
13165 obj->JointInternal.MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
13166 }
13167
13168 b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *
13169 obj->JointInternal.PositionLimitsInternal->size[1];
13170 obj->JointInternal.PositionLimitsInternal->size[0] = 1;
13171 obj->JointInternal.PositionLimitsInternal->size[1] = 2;
13172 cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,
13173 b_kstr);
13174 for (b_kstr = 0; b_kstr < 2; b_kstr++) {
13175 obj->JointInternal.PositionLimitsInternal->data[b_kstr] = poslim_data[b_kstr];
13176 }
13177
13178 b_kstr = obj->JointInternal.HomePositionInternal->size[0];
13179 obj->JointInternal.HomePositionInternal->size[0] = 1;
13180 cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,
13181 b_kstr);
13182 for (b_kstr = 0; b_kstr < 1; b_kstr++) {
13183 obj->JointInternal.HomePositionInternal->data[0] = 0.0;
13184 }
13185
13186 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
13187 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_7[b_kstr];
13188 }
13189
13190 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
13191 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_8[b_kstr];
13192 }
13193
13194 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
13195 obj->JointInternal.MotionSubspace->size[1];
13196 obj->JointInternal.MotionSubspace->size[0] = 6;
13197 obj->JointInternal.MotionSubspace->size[1] = 1;
13198 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
13199 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
13200 obj->JointInternal.MotionSubspace->data[b_kstr] = tmp_9[b_kstr];
13201 }
13202
13203 obj->JointInternal.InTree = true;
13204 b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *
13205 obj->JointInternal.PositionLimitsInternal->size[1];
13206 obj->JointInternal.PositionLimitsInternal->size[0] = 1;
13207 obj->JointInternal.PositionLimitsInternal->size[1] = 2;
13208 cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,
13209 b_kstr);
13210 obj->JointInternal.PositionLimitsInternal->data[0] = -3.10668606855;
13211 obj->JointInternal.PositionLimitsInternal->data
13212 [obj->JointInternal.PositionLimitsInternal->size[0]] = 3.10668606855;
13213 obj->JointInternal.JointAxisInternal[0] = 0.0;
13214 obj->JointInternal.JointAxisInternal[1] = 0.0;
13215 obj->JointInternal.JointAxisInternal[2] = 1.0;
13216 b_kstr = obj->JointInternal.HomePositionInternal->size[0];
13217 obj->JointInternal.HomePositionInternal->size[0] = 1;
13218 cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,
13219 b_kstr);
13220 obj->JointInternal.HomePositionInternal->data[0] = 0.0;
13221 return b_obj;
13222}
13223
13224static v_robotics_manip_internal_Rig_T *RigidBody_RigidBody_astwhqf2azt
13225 (v_robotics_manip_internal_Rig_T *obj)
13226{
13227 v_robotics_manip_internal_Rig_T *b_obj;
13228 int8_T msubspace_data[36];
13229 real_T poslim_data[12];
13230 emxArray_char_T_cartesian_tra_T *switch_expression;
13231 boolean_T b_bool;
13232 int32_T b_kstr;
13233 char_T b[8];
13234 char_T b_0[9];
13235 int32_T loop_ub;
13236 int8_T tmp[6];
13237 static const char_T tmp_0[10] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
13238 '2' };
13239
13240 static const real_T tmp_1[9] = { 0.012452446533447339, 0.00097164648860403489,
13241 -0.0012079400901817208, 0.00097164648860403489, 0.0077631932790618,
13242 0.0060529024261622953, -0.0012079400901817208, 0.0060529024261622953,
13243 0.0051581160550583753 };
13244
13245 static const real_T tmp_2[36] = { 0.012452446533447339, 0.00097164648860403489,
13246 -0.0012079400901817208, 0.0, 0.0036112478581453288, 0.0024995324199659588,
13247 0.00097164648860403489, 0.0077631932790618, 0.0060529024261622953,
13248 -0.0036112478581453288, 0.0, 0.0012907531029494371, -0.0012079400901817208,
13249 0.0060529024261622953, 0.0051581160550583753, -0.0024995324199659588,
13250 -0.0012907531029494371, 0.0, 0.0, -0.0036112478581453288,
13251 -0.0024995324199659588, 0.0785942338762368, 0.0, 0.0, 0.0036112478581453288,
13252 0.0, -0.0012907531029494371, 0.0, 0.0785942338762368, 0.0,
13253 0.0024995324199659588, 0.0012907531029494371, 0.0, 0.0, 0.0,
13254 0.0785942338762368 };
13255
13256 static const int8_T tmp_3[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,
13257 1 };
13258
13259 static const char_T tmp_4[11] = { 'e', 'd', 'o', '_', 'j', 'o', 'i', 'n', 't',
13260 '_', '2' };
13261
13262 static const char_T tmp_5[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
13263
13264 static const char_T tmp_6[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
13265
13266 static const real_T tmp_7[16] = { 1.0, 0.0, -0.0, 0.0, 0.0,
13267 4.8965888601467475E-12, 1.0, 0.0, 0.0, -1.0, 4.8965888601467475E-12, 0.0,
13268 0.0, 0.0, 0.0, 1.0 };
13269
13270 static const real_T tmp_8[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
13271 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
13272
13273 static const real_T tmp_9[36] = { 0.0, 0.0, -1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
13274 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
13275 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
13276
13277 int32_T exitg1;
13278 b_obj = obj;
13279 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
13280 obj->NameInternal->size[0] = 1;
13281 obj->NameInternal->size[1] = 10;
13282 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);
13283 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
13284 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
13285 }
13286
13287 obj->ParentIndex = 2.0;
13288 obj->MassInternal = 0.0785942338762368;
13289 obj->CenterOfMassInternal[0] = -0.016423;
13290 obj->CenterOfMassInternal[1] = 0.031803;
13291 obj->CenterOfMassInternal[2] = -0.045948;
13292 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
13293 obj->InertiaInternal[b_kstr] = tmp_1[b_kstr];
13294 }
13295
13296 for (b_kstr = 0; b_kstr < 36; b_kstr++) {
13297 obj->SpatialInertia[b_kstr] = tmp_2[b_kstr];
13298 }
13299
13300 obj->JointInternal.InTree = false;
13301 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
13302 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_3[b_kstr];
13303 }
13304
13305 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
13306 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_3[b_kstr];
13307 }
13308
13309 b_kstr = obj->JointInternal.NameInternal->size[0] *
13310 obj->JointInternal.NameInternal->size[1];
13311 obj->JointInternal.NameInternal->size[0] = 1;
13312 obj->JointInternal.NameInternal->size[1] = 11;
13313 cartes_emxEnsureCapacity_char_T(obj->JointInternal.NameInternal, b_kstr);
13314 for (b_kstr = 0; b_kstr < 11; b_kstr++) {
13315 obj->JointInternal.NameInternal->data[b_kstr] = tmp_4[b_kstr];
13316 }
13317
13318 b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];
13319 obj->JointInternal.Type->size[0] = 1;
13320 obj->JointInternal.Type->size[1] = 8;
13321 cartes_emxEnsureCapacity_char_T(obj->JointInternal.Type, b_kstr);
13322 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
13323 obj->JointInternal.Type->data[b_kstr] = tmp_5[b_kstr];
13324 }
13325
13326 cartesian_trajec_emxInit_char_T(&switch_expression, 2);
13327 b_kstr = switch_expression->size[0] * switch_expression->size[1];
13328 switch_expression->size[0] = 1;
13329 switch_expression->size[1] = obj->JointInternal.Type->size[1];
13330 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
13331 loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]
13332 - 1;
13333 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
13334 switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];
13335 }
13336
13337 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
13338 b[b_kstr] = tmp_5[b_kstr];
13339 }
13340
13341 b_bool = false;
13342 if (switch_expression->size[1] == 8) {
13343 b_kstr = 1;
13344 do {
13345 exitg1 = 0;
13346 if (b_kstr - 1 < 8) {
13347 loop_ub = b_kstr - 1;
13348 if (switch_expression->data[loop_ub] != b[loop_ub]) {
13349 exitg1 = 1;
13350 } else {
13351 b_kstr++;
13352 }
13353 } else {
13354 b_bool = true;
13355 exitg1 = 1;
13356 }
13357 } while (exitg1 == 0);
13358 }
13359
13360 if (b_bool) {
13361 b_kstr = 0;
13362 } else {
13363 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
13364 b_0[b_kstr] = tmp_6[b_kstr];
13365 }
13366
13367 b_bool = false;
13368 if (switch_expression->size[1] == 9) {
13369 b_kstr = 1;
13370 do {
13371 exitg1 = 0;
13372 if (b_kstr - 1 < 9) {
13373 loop_ub = b_kstr - 1;
13374 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
13375 exitg1 = 1;
13376 } else {
13377 b_kstr++;
13378 }
13379 } else {
13380 b_bool = true;
13381 exitg1 = 1;
13382 }
13383 } while (exitg1 == 0);
13384 }
13385
13386 if (b_bool) {
13387 b_kstr = 1;
13388 } else {
13389 b_kstr = -1;
13390 }
13391 }
13392
13393 cartesian_trajec_emxFree_char_T(&switch_expression);
13394 switch (b_kstr) {
13395 case 0:
13396 tmp[0] = 0;
13397 tmp[1] = 0;
13398 tmp[2] = 1;
13399 tmp[3] = 0;
13400 tmp[4] = 0;
13401 tmp[5] = 0;
13402 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
13403 msubspace_data[b_kstr] = tmp[b_kstr];
13404 }
13405
13406 poslim_data[0] = -3.1415926535897931;
13407 poslim_data[1] = 3.1415926535897931;
13408 obj->JointInternal.VelocityNumber = 1.0;
13409 obj->JointInternal.PositionNumber = 1.0;
13410 obj->JointInternal.JointAxisInternal[0] = 0.0;
13411 obj->JointInternal.JointAxisInternal[1] = 0.0;
13412 obj->JointInternal.JointAxisInternal[2] = 1.0;
13413 break;
13414
13415 case 1:
13416 tmp[0] = 0;
13417 tmp[1] = 0;
13418 tmp[2] = 0;
13419 tmp[3] = 0;
13420 tmp[4] = 0;
13421 tmp[5] = 1;
13422 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
13423 msubspace_data[b_kstr] = tmp[b_kstr];
13424 }
13425
13426 poslim_data[0] = -0.5;
13427 poslim_data[1] = 0.5;
13428 obj->JointInternal.VelocityNumber = 1.0;
13429 obj->JointInternal.PositionNumber = 1.0;
13430 obj->JointInternal.JointAxisInternal[0] = 0.0;
13431 obj->JointInternal.JointAxisInternal[1] = 0.0;
13432 obj->JointInternal.JointAxisInternal[2] = 1.0;
13433 break;
13434
13435 default:
13436 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
13437 msubspace_data[b_kstr] = 0;
13438 }
13439
13440 poslim_data[0] = 0.0;
13441 poslim_data[1] = 0.0;
13442 obj->JointInternal.VelocityNumber = 0.0;
13443 obj->JointInternal.PositionNumber = 0.0;
13444 obj->JointInternal.JointAxisInternal[0] = 0.0;
13445 obj->JointInternal.JointAxisInternal[1] = 0.0;
13446 obj->JointInternal.JointAxisInternal[2] = 0.0;
13447 break;
13448 }
13449
13450 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
13451 obj->JointInternal.MotionSubspace->size[1];
13452 obj->JointInternal.MotionSubspace->size[0] = 6;
13453 obj->JointInternal.MotionSubspace->size[1] = 1;
13454 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
13455 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
13456 obj->JointInternal.MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
13457 }
13458
13459 b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *
13460 obj->JointInternal.PositionLimitsInternal->size[1];
13461 obj->JointInternal.PositionLimitsInternal->size[0] = 1;
13462 obj->JointInternal.PositionLimitsInternal->size[1] = 2;
13463 cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,
13464 b_kstr);
13465 for (b_kstr = 0; b_kstr < 2; b_kstr++) {
13466 obj->JointInternal.PositionLimitsInternal->data[b_kstr] = poslim_data[b_kstr];
13467 }
13468
13469 b_kstr = obj->JointInternal.HomePositionInternal->size[0];
13470 obj->JointInternal.HomePositionInternal->size[0] = 1;
13471 cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,
13472 b_kstr);
13473 for (b_kstr = 0; b_kstr < 1; b_kstr++) {
13474 obj->JointInternal.HomePositionInternal->data[0] = 0.0;
13475 }
13476
13477 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
13478 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_7[b_kstr];
13479 }
13480
13481 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
13482 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_8[b_kstr];
13483 }
13484
13485 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
13486 obj->JointInternal.MotionSubspace->size[1];
13487 obj->JointInternal.MotionSubspace->size[0] = 6;
13488 obj->JointInternal.MotionSubspace->size[1] = 1;
13489 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
13490 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
13491 obj->JointInternal.MotionSubspace->data[b_kstr] = tmp_9[b_kstr];
13492 }
13493
13494 obj->JointInternal.InTree = true;
13495 b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *
13496 obj->JointInternal.PositionLimitsInternal->size[1];
13497 obj->JointInternal.PositionLimitsInternal->size[0] = 1;
13498 obj->JointInternal.PositionLimitsInternal->size[1] = 2;
13499 cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,
13500 b_kstr);
13501 obj->JointInternal.PositionLimitsInternal->data[0] = -1.71042266695;
13502 obj->JointInternal.PositionLimitsInternal->data
13503 [obj->JointInternal.PositionLimitsInternal->size[0]] = 1.71042266695;
13504 obj->JointInternal.JointAxisInternal[0] = 0.0;
13505 obj->JointInternal.JointAxisInternal[1] = 0.0;
13506 obj->JointInternal.JointAxisInternal[2] = -1.0;
13507 b_kstr = obj->JointInternal.HomePositionInternal->size[0];
13508 obj->JointInternal.HomePositionInternal->size[0] = 1;
13509 cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,
13510 b_kstr);
13511 obj->JointInternal.HomePositionInternal->data[0] = 0.0;
13512 return b_obj;
13513}
13514
13515static v_robotics_manip_internal_Rig_T *RigidBody_RigidBod_astwhqf2aztt
13516 (v_robotics_manip_internal_Rig_T *obj)
13517{
13518 v_robotics_manip_internal_Rig_T *b_obj;
13519 int8_T msubspace_data[36];
13520 real_T poslim_data[12];
13521 emxArray_char_T_cartesian_tra_T *switch_expression;
13522 boolean_T b_bool;
13523 int32_T b_kstr;
13524 char_T b[8];
13525 char_T b_0[9];
13526 int32_T loop_ub;
13527 int8_T tmp[6];
13528 static const char_T tmp_0[10] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
13529 '3' };
13530
13531 static const real_T tmp_1[9] = { 0.0123976159829631, 0.00022039108420015264,
13532 2.1332825710116445E-6, 0.00022039108420015264, 0.00014803877895089843,
13533 -9.2077339045129963E-5, 2.1332825710116445E-6, -9.2077339045129963E-5,
13534 0.012477575138803739 };
13535
13536 static const real_T tmp_2[36] = { 0.0123976159829631, 0.00022039108420015264,
13537 2.1332825710116445E-6, 0.0, -2.56217202436532E-5, 0.0010295844637787021,
13538 0.00022039108420015264, 0.00014803877895089843, -9.2077339045129963E-5,
13539 2.56217202436532E-5, 0.0, 0.0024737535112545539, 2.1332825710116445E-6,
13540 -9.2077339045129963E-5, 0.012477575138803739, -0.0010295844637787021,
13541 -0.0024737535112545539, 0.0, 0.0, 2.56217202436532E-5,
13542 -0.0010295844637787021, 0.0785942338762368, 0.0, 0.0, -2.56217202436532E-5,
13543 0.0, -0.0024737535112545539, 0.0, 0.0785942338762368, 0.0,
13544 0.0010295844637787021, 0.0024737535112545539, 0.0, 0.0, 0.0,
13545 0.0785942338762368 };
13546
13547 static const int8_T tmp_3[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,
13548 1 };
13549
13550 static const char_T tmp_4[11] = { 'e', 'd', 'o', '_', 'j', 'o', 'i', 'n', 't',
13551 '_', '3' };
13552
13553 static const char_T tmp_5[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
13554
13555 static const char_T tmp_6[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
13556
13557 static const real_T tmp_7[16] = { 1.0, 2.0682310711021444E-13,
13558 2.0682310711021444E-13, 0.0, 2.0682310711021444E-13, -1.0, -0.0, 0.0,
13559 2.0682310711021444E-13, 4.2775797634723234E-26, -1.0, 0.0, 0.0, 0.2105, 0.0,
13560 1.0 };
13561
13562 static const real_T tmp_8[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
13563 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
13564
13565 static const real_T tmp_9[36] = { 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
13566 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
13567 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
13568
13569 int32_T exitg1;
13570 b_obj = obj;
13571 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
13572 obj->NameInternal->size[0] = 1;
13573 obj->NameInternal->size[1] = 10;
13574 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);
13575 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
13576 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
13577 }
13578
13579 obj->ParentIndex = 3.0;
13580 obj->MassInternal = 0.0785942338762368;
13581 obj->CenterOfMassInternal[0] = -0.031475;
13582 obj->CenterOfMassInternal[1] = 0.0131;
13583 obj->CenterOfMassInternal[2] = 0.000326;
13584 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
13585 obj->InertiaInternal[b_kstr] = tmp_1[b_kstr];
13586 }
13587
13588 for (b_kstr = 0; b_kstr < 36; b_kstr++) {
13589 obj->SpatialInertia[b_kstr] = tmp_2[b_kstr];
13590 }
13591
13592 obj->JointInternal.InTree = false;
13593 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
13594 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_3[b_kstr];
13595 }
13596
13597 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
13598 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_3[b_kstr];
13599 }
13600
13601 b_kstr = obj->JointInternal.NameInternal->size[0] *
13602 obj->JointInternal.NameInternal->size[1];
13603 obj->JointInternal.NameInternal->size[0] = 1;
13604 obj->JointInternal.NameInternal->size[1] = 11;
13605 cartes_emxEnsureCapacity_char_T(obj->JointInternal.NameInternal, b_kstr);
13606 for (b_kstr = 0; b_kstr < 11; b_kstr++) {
13607 obj->JointInternal.NameInternal->data[b_kstr] = tmp_4[b_kstr];
13608 }
13609
13610 b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];
13611 obj->JointInternal.Type->size[0] = 1;
13612 obj->JointInternal.Type->size[1] = 8;
13613 cartes_emxEnsureCapacity_char_T(obj->JointInternal.Type, b_kstr);
13614 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
13615 obj->JointInternal.Type->data[b_kstr] = tmp_5[b_kstr];
13616 }
13617
13618 cartesian_trajec_emxInit_char_T(&switch_expression, 2);
13619 b_kstr = switch_expression->size[0] * switch_expression->size[1];
13620 switch_expression->size[0] = 1;
13621 switch_expression->size[1] = obj->JointInternal.Type->size[1];
13622 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
13623 loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]
13624 - 1;
13625 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
13626 switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];
13627 }
13628
13629 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
13630 b[b_kstr] = tmp_5[b_kstr];
13631 }
13632
13633 b_bool = false;
13634 if (switch_expression->size[1] == 8) {
13635 b_kstr = 1;
13636 do {
13637 exitg1 = 0;
13638 if (b_kstr - 1 < 8) {
13639 loop_ub = b_kstr - 1;
13640 if (switch_expression->data[loop_ub] != b[loop_ub]) {
13641 exitg1 = 1;
13642 } else {
13643 b_kstr++;
13644 }
13645 } else {
13646 b_bool = true;
13647 exitg1 = 1;
13648 }
13649 } while (exitg1 == 0);
13650 }
13651
13652 if (b_bool) {
13653 b_kstr = 0;
13654 } else {
13655 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
13656 b_0[b_kstr] = tmp_6[b_kstr];
13657 }
13658
13659 b_bool = false;
13660 if (switch_expression->size[1] == 9) {
13661 b_kstr = 1;
13662 do {
13663 exitg1 = 0;
13664 if (b_kstr - 1 < 9) {
13665 loop_ub = b_kstr - 1;
13666 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
13667 exitg1 = 1;
13668 } else {
13669 b_kstr++;
13670 }
13671 } else {
13672 b_bool = true;
13673 exitg1 = 1;
13674 }
13675 } while (exitg1 == 0);
13676 }
13677
13678 if (b_bool) {
13679 b_kstr = 1;
13680 } else {
13681 b_kstr = -1;
13682 }
13683 }
13684
13685 cartesian_trajec_emxFree_char_T(&switch_expression);
13686 switch (b_kstr) {
13687 case 0:
13688 tmp[0] = 0;
13689 tmp[1] = 0;
13690 tmp[2] = 1;
13691 tmp[3] = 0;
13692 tmp[4] = 0;
13693 tmp[5] = 0;
13694 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
13695 msubspace_data[b_kstr] = tmp[b_kstr];
13696 }
13697
13698 poslim_data[0] = -3.1415926535897931;
13699 poslim_data[1] = 3.1415926535897931;
13700 obj->JointInternal.VelocityNumber = 1.0;
13701 obj->JointInternal.PositionNumber = 1.0;
13702 obj->JointInternal.JointAxisInternal[0] = 0.0;
13703 obj->JointInternal.JointAxisInternal[1] = 0.0;
13704 obj->JointInternal.JointAxisInternal[2] = 1.0;
13705 break;
13706
13707 case 1:
13708 tmp[0] = 0;
13709 tmp[1] = 0;
13710 tmp[2] = 0;
13711 tmp[3] = 0;
13712 tmp[4] = 0;
13713 tmp[5] = 1;
13714 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
13715 msubspace_data[b_kstr] = tmp[b_kstr];
13716 }
13717
13718 poslim_data[0] = -0.5;
13719 poslim_data[1] = 0.5;
13720 obj->JointInternal.VelocityNumber = 1.0;
13721 obj->JointInternal.PositionNumber = 1.0;
13722 obj->JointInternal.JointAxisInternal[0] = 0.0;
13723 obj->JointInternal.JointAxisInternal[1] = 0.0;
13724 obj->JointInternal.JointAxisInternal[2] = 1.0;
13725 break;
13726
13727 default:
13728 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
13729 msubspace_data[b_kstr] = 0;
13730 }
13731
13732 poslim_data[0] = 0.0;
13733 poslim_data[1] = 0.0;
13734 obj->JointInternal.VelocityNumber = 0.0;
13735 obj->JointInternal.PositionNumber = 0.0;
13736 obj->JointInternal.JointAxisInternal[0] = 0.0;
13737 obj->JointInternal.JointAxisInternal[1] = 0.0;
13738 obj->JointInternal.JointAxisInternal[2] = 0.0;
13739 break;
13740 }
13741
13742 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
13743 obj->JointInternal.MotionSubspace->size[1];
13744 obj->JointInternal.MotionSubspace->size[0] = 6;
13745 obj->JointInternal.MotionSubspace->size[1] = 1;
13746 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
13747 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
13748 obj->JointInternal.MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
13749 }
13750
13751 b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *
13752 obj->JointInternal.PositionLimitsInternal->size[1];
13753 obj->JointInternal.PositionLimitsInternal->size[0] = 1;
13754 obj->JointInternal.PositionLimitsInternal->size[1] = 2;
13755 cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,
13756 b_kstr);
13757 for (b_kstr = 0; b_kstr < 2; b_kstr++) {
13758 obj->JointInternal.PositionLimitsInternal->data[b_kstr] = poslim_data[b_kstr];
13759 }
13760
13761 b_kstr = obj->JointInternal.HomePositionInternal->size[0];
13762 obj->JointInternal.HomePositionInternal->size[0] = 1;
13763 cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,
13764 b_kstr);
13765 for (b_kstr = 0; b_kstr < 1; b_kstr++) {
13766 obj->JointInternal.HomePositionInternal->data[0] = 0.0;
13767 }
13768
13769 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
13770 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_7[b_kstr];
13771 }
13772
13773 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
13774 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_8[b_kstr];
13775 }
13776
13777 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
13778 obj->JointInternal.MotionSubspace->size[1];
13779 obj->JointInternal.MotionSubspace->size[0] = 6;
13780 obj->JointInternal.MotionSubspace->size[1] = 1;
13781 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
13782 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
13783 obj->JointInternal.MotionSubspace->data[b_kstr] = tmp_9[b_kstr];
13784 }
13785
13786 obj->JointInternal.InTree = true;
13787 b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *
13788 obj->JointInternal.PositionLimitsInternal->size[1];
13789 obj->JointInternal.PositionLimitsInternal->size[0] = 1;
13790 obj->JointInternal.PositionLimitsInternal->size[1] = 2;
13791 cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,
13792 b_kstr);
13793 obj->JointInternal.PositionLimitsInternal->data[0] = -1.71042266695;
13794 obj->JointInternal.PositionLimitsInternal->data
13795 [obj->JointInternal.PositionLimitsInternal->size[0]] = 1.71042266695;
13796 obj->JointInternal.JointAxisInternal[0] = 0.0;
13797 obj->JointInternal.JointAxisInternal[1] = 0.0;
13798 obj->JointInternal.JointAxisInternal[2] = 1.0;
13799 b_kstr = obj->JointInternal.HomePositionInternal->size[0];
13800 obj->JointInternal.HomePositionInternal->size[0] = 1;
13801 cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,
13802 b_kstr);
13803 obj->JointInternal.HomePositionInternal->data[0] = 0.0;
13804 return b_obj;
13805}
13806
13807static v_robotics_manip_internal_Rig_T *RigidBody_RigidBo_astwhqf2azttx
13808 (v_robotics_manip_internal_Rig_T *obj)
13809{
13810 v_robotics_manip_internal_Rig_T *b_obj;
13811 int8_T msubspace_data[36];
13812 real_T poslim_data[12];
13813 emxArray_char_T_cartesian_tra_T *switch_expression;
13814 boolean_T b_bool;
13815 int32_T b_kstr;
13816 char_T b[8];
13817 char_T b_0[9];
13818 int32_T loop_ub;
13819 int8_T tmp[6];
13820 static const char_T tmp_0[10] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
13821 '4' };
13822
13823 static const real_T tmp_1[9] = { 0.0123990349928174, -2.7766271471639167E-6,
13824 0.00022466935228286869, -2.7766271471639167E-6, 0.012491487094789458,
13825 9.2330220708293281E-5, 0.00022466935228286869, 9.2330220708293281E-5,
13826 0.00016056153744711284 };
13827
13828 static const real_T tmp_2[36] = { 0.0123990349928174, -2.7766271471639167E-6,
13829 0.00022466935228286869, 0.0, 0.0010818496293063995, 4.275526322867282E-5,
13830 -2.7766271471639167E-6, 0.012491487094789458, 9.2330220708293281E-5,
13831 -0.0010818496293063995, 0.0, -0.0026650518765093138, 0.00022466935228286869,
13832 9.2330220708293281E-5, 0.00016056153744711284, -4.275526322867282E-5,
13833 0.0026650518765093138, 0.0, 0.0, -0.0010818496293063995,
13834 -4.275526322867282E-5, 0.0785942338762368, 0.0, 0.0, 0.0010818496293063995,
13835 0.0, 0.0026650518765093138, 0.0, 0.0785942338762368, 0.0,
13836 4.275526322867282E-5, -0.0026650518765093138, 0.0, 0.0, 0.0,
13837 0.0785942338762368 };
13838
13839 static const int8_T tmp_3[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,
13840 1 };
13841
13842 static const char_T tmp_4[11] = { 'e', 'd', 'o', '_', 'j', 'o', 'i', 'n', 't',
13843 '_', '4' };
13844
13845 static const char_T tmp_5[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
13846
13847 static const char_T tmp_6[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
13848
13849 static const real_T tmp_7[16] = { 1.0, 0.0, -0.0, 0.0, 0.0,
13850 4.8965888601467475E-12, 1.0, 0.0, 0.0, -1.0, 4.8965888601467475E-12, 0.0,
13851 0.0, -0.268, 0.0, 1.0 };
13852
13853 static const real_T tmp_8[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
13854 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
13855
13856 static const real_T tmp_9[36] = { 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
13857 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
13858 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
13859
13860 int32_T exitg1;
13861 b_obj = obj;
13862 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
13863 obj->NameInternal->size[0] = 1;
13864 obj->NameInternal->size[1] = 10;
13865 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);
13866 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
13867 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
13868 }
13869
13870 obj->ParentIndex = 4.0;
13871 obj->MassInternal = 0.0785942338762368;
13872 obj->CenterOfMassInternal[0] = 0.033909;
13873 obj->CenterOfMassInternal[1] = 0.000544;
13874 obj->CenterOfMassInternal[2] = -0.013765;
13875 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
13876 obj->InertiaInternal[b_kstr] = tmp_1[b_kstr];
13877 }
13878
13879 for (b_kstr = 0; b_kstr < 36; b_kstr++) {
13880 obj->SpatialInertia[b_kstr] = tmp_2[b_kstr];
13881 }
13882
13883 obj->JointInternal.InTree = false;
13884 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
13885 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_3[b_kstr];
13886 }
13887
13888 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
13889 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_3[b_kstr];
13890 }
13891
13892 b_kstr = obj->JointInternal.NameInternal->size[0] *
13893 obj->JointInternal.NameInternal->size[1];
13894 obj->JointInternal.NameInternal->size[0] = 1;
13895 obj->JointInternal.NameInternal->size[1] = 11;
13896 cartes_emxEnsureCapacity_char_T(obj->JointInternal.NameInternal, b_kstr);
13897 for (b_kstr = 0; b_kstr < 11; b_kstr++) {
13898 obj->JointInternal.NameInternal->data[b_kstr] = tmp_4[b_kstr];
13899 }
13900
13901 b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];
13902 obj->JointInternal.Type->size[0] = 1;
13903 obj->JointInternal.Type->size[1] = 8;
13904 cartes_emxEnsureCapacity_char_T(obj->JointInternal.Type, b_kstr);
13905 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
13906 obj->JointInternal.Type->data[b_kstr] = tmp_5[b_kstr];
13907 }
13908
13909 cartesian_trajec_emxInit_char_T(&switch_expression, 2);
13910 b_kstr = switch_expression->size[0] * switch_expression->size[1];
13911 switch_expression->size[0] = 1;
13912 switch_expression->size[1] = obj->JointInternal.Type->size[1];
13913 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
13914 loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]
13915 - 1;
13916 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
13917 switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];
13918 }
13919
13920 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
13921 b[b_kstr] = tmp_5[b_kstr];
13922 }
13923
13924 b_bool = false;
13925 if (switch_expression->size[1] == 8) {
13926 b_kstr = 1;
13927 do {
13928 exitg1 = 0;
13929 if (b_kstr - 1 < 8) {
13930 loop_ub = b_kstr - 1;
13931 if (switch_expression->data[loop_ub] != b[loop_ub]) {
13932 exitg1 = 1;
13933 } else {
13934 b_kstr++;
13935 }
13936 } else {
13937 b_bool = true;
13938 exitg1 = 1;
13939 }
13940 } while (exitg1 == 0);
13941 }
13942
13943 if (b_bool) {
13944 b_kstr = 0;
13945 } else {
13946 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
13947 b_0[b_kstr] = tmp_6[b_kstr];
13948 }
13949
13950 b_bool = false;
13951 if (switch_expression->size[1] == 9) {
13952 b_kstr = 1;
13953 do {
13954 exitg1 = 0;
13955 if (b_kstr - 1 < 9) {
13956 loop_ub = b_kstr - 1;
13957 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
13958 exitg1 = 1;
13959 } else {
13960 b_kstr++;
13961 }
13962 } else {
13963 b_bool = true;
13964 exitg1 = 1;
13965 }
13966 } while (exitg1 == 0);
13967 }
13968
13969 if (b_bool) {
13970 b_kstr = 1;
13971 } else {
13972 b_kstr = -1;
13973 }
13974 }
13975
13976 cartesian_trajec_emxFree_char_T(&switch_expression);
13977 switch (b_kstr) {
13978 case 0:
13979 tmp[0] = 0;
13980 tmp[1] = 0;
13981 tmp[2] = 1;
13982 tmp[3] = 0;
13983 tmp[4] = 0;
13984 tmp[5] = 0;
13985 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
13986 msubspace_data[b_kstr] = tmp[b_kstr];
13987 }
13988
13989 poslim_data[0] = -3.1415926535897931;
13990 poslim_data[1] = 3.1415926535897931;
13991 obj->JointInternal.VelocityNumber = 1.0;
13992 obj->JointInternal.PositionNumber = 1.0;
13993 obj->JointInternal.JointAxisInternal[0] = 0.0;
13994 obj->JointInternal.JointAxisInternal[1] = 0.0;
13995 obj->JointInternal.JointAxisInternal[2] = 1.0;
13996 break;
13997
13998 case 1:
13999 tmp[0] = 0;
14000 tmp[1] = 0;
14001 tmp[2] = 0;
14002 tmp[3] = 0;
14003 tmp[4] = 0;
14004 tmp[5] = 1;
14005 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
14006 msubspace_data[b_kstr] = tmp[b_kstr];
14007 }
14008
14009 poslim_data[0] = -0.5;
14010 poslim_data[1] = 0.5;
14011 obj->JointInternal.VelocityNumber = 1.0;
14012 obj->JointInternal.PositionNumber = 1.0;
14013 obj->JointInternal.JointAxisInternal[0] = 0.0;
14014 obj->JointInternal.JointAxisInternal[1] = 0.0;
14015 obj->JointInternal.JointAxisInternal[2] = 1.0;
14016 break;
14017
14018 default:
14019 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
14020 msubspace_data[b_kstr] = 0;
14021 }
14022
14023 poslim_data[0] = 0.0;
14024 poslim_data[1] = 0.0;
14025 obj->JointInternal.VelocityNumber = 0.0;
14026 obj->JointInternal.PositionNumber = 0.0;
14027 obj->JointInternal.JointAxisInternal[0] = 0.0;
14028 obj->JointInternal.JointAxisInternal[1] = 0.0;
14029 obj->JointInternal.JointAxisInternal[2] = 0.0;
14030 break;
14031 }
14032
14033 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
14034 obj->JointInternal.MotionSubspace->size[1];
14035 obj->JointInternal.MotionSubspace->size[0] = 6;
14036 obj->JointInternal.MotionSubspace->size[1] = 1;
14037 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
14038 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
14039 obj->JointInternal.MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
14040 }
14041
14042 b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *
14043 obj->JointInternal.PositionLimitsInternal->size[1];
14044 obj->JointInternal.PositionLimitsInternal->size[0] = 1;
14045 obj->JointInternal.PositionLimitsInternal->size[1] = 2;
14046 cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,
14047 b_kstr);
14048 for (b_kstr = 0; b_kstr < 2; b_kstr++) {
14049 obj->JointInternal.PositionLimitsInternal->data[b_kstr] = poslim_data[b_kstr];
14050 }
14051
14052 b_kstr = obj->JointInternal.HomePositionInternal->size[0];
14053 obj->JointInternal.HomePositionInternal->size[0] = 1;
14054 cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,
14055 b_kstr);
14056 for (b_kstr = 0; b_kstr < 1; b_kstr++) {
14057 obj->JointInternal.HomePositionInternal->data[0] = 0.0;
14058 }
14059
14060 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
14061 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_7[b_kstr];
14062 }
14063
14064 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
14065 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_8[b_kstr];
14066 }
14067
14068 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
14069 obj->JointInternal.MotionSubspace->size[1];
14070 obj->JointInternal.MotionSubspace->size[0] = 6;
14071 obj->JointInternal.MotionSubspace->size[1] = 1;
14072 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
14073 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
14074 obj->JointInternal.MotionSubspace->data[b_kstr] = tmp_9[b_kstr];
14075 }
14076
14077 obj->JointInternal.InTree = true;
14078 b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *
14079 obj->JointInternal.PositionLimitsInternal->size[1];
14080 obj->JointInternal.PositionLimitsInternal->size[0] = 1;
14081 obj->JointInternal.PositionLimitsInternal->size[1] = 2;
14082 cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,
14083 b_kstr);
14084 obj->JointInternal.PositionLimitsInternal->data[0] = -3.10668606855;
14085 obj->JointInternal.PositionLimitsInternal->data
14086 [obj->JointInternal.PositionLimitsInternal->size[0]] = 3.10668606855;
14087 obj->JointInternal.JointAxisInternal[0] = 0.0;
14088 obj->JointInternal.JointAxisInternal[1] = 0.0;
14089 obj->JointInternal.JointAxisInternal[2] = 1.0;
14090 b_kstr = obj->JointInternal.HomePositionInternal->size[0];
14091 obj->JointInternal.HomePositionInternal->size[0] = 1;
14092 cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,
14093 b_kstr);
14094 obj->JointInternal.HomePositionInternal->data[0] = 0.0;
14095 return b_obj;
14096}
14097
14098static v_robotics_manip_internal_Rig_T *RigidBody_RigidB_astwhqf2azttxa
14099 (v_robotics_manip_internal_Rig_T *obj)
14100{
14101 v_robotics_manip_internal_Rig_T *b_obj;
14102 int8_T msubspace_data[36];
14103 real_T poslim_data[12];
14104 emxArray_char_T_cartesian_tra_T *switch_expression;
14105 boolean_T b_bool;
14106 int32_T b_kstr;
14107 char_T b[8];
14108 char_T b_0[9];
14109 int32_T loop_ub;
14110 int8_T tmp[6];
14111 static const char_T tmp_0[10] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
14112 '5' };
14113
14114 static const real_T tmp_1[9] = { 0.012440329403329006, 2.388185677857016E-6,
14115 0.00012602126519218373, 2.388185677857016E-6, 0.012510746127660349,
14116 -9.077919321137075E-5, 0.00012602126519218373, -9.077919321137075E-5,
14117 0.00013851261456175015 };
14118
14119 static const real_T tmp_2[36] = { 0.012440329403329006, 2.388185677857016E-6,
14120 0.00012602126519218373, 0.0, -0.0021015312196166957, -3.5996159115316455E-5,
14121 2.388185677857016E-6, 0.012510746127660349, -9.077919321137075E-5,
14122 0.0021015312196166957, 0.0, -0.0023173509858408423, 0.00012602126519218373,
14123 -9.077919321137075E-5, 0.00013851261456175015, 3.5996159115316455E-5,
14124 0.0023173509858408423, 0.0, 0.0, 0.0021015312196166957,
14125 3.5996159115316455E-5, 0.0785942338762368, 0.0, 0.0, -0.0021015312196166957,
14126 0.0, 0.0023173509858408423, 0.0, 0.0785942338762368, 0.0,
14127 -3.5996159115316455E-5, -0.0023173509858408423, 0.0, 0.0, 0.0,
14128 0.0785942338762368 };
14129
14130 static const int8_T tmp_3[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,
14131 1 };
14132
14133 static const char_T tmp_4[11] = { 'e', 'd', 'o', '_', 'j', 'o', 'i', 'n', 't',
14134 '_', '5' };
14135
14136 static const char_T tmp_5[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
14137
14138 static const char_T tmp_6[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
14139
14140 static const real_T tmp_7[16] = { 1.0, 0.0, -0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
14141 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
14142
14143 static const real_T tmp_8[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
14144 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
14145
14146 static const real_T tmp_9[36] = { 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
14147 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
14148 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
14149
14150 int32_T exitg1;
14151 b_obj = obj;
14152 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
14153 obj->NameInternal->size[0] = 1;
14154 obj->NameInternal->size[1] = 10;
14155 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);
14156 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
14157 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
14158 }
14159
14160 obj->ParentIndex = 5.0;
14161 obj->MassInternal = 0.0785942338762368;
14162 obj->CenterOfMassInternal[0] = 0.029485;
14163 obj->CenterOfMassInternal[1] = -0.000458;
14164 obj->CenterOfMassInternal[2] = 0.026739;
14165 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
14166 obj->InertiaInternal[b_kstr] = tmp_1[b_kstr];
14167 }
14168
14169 for (b_kstr = 0; b_kstr < 36; b_kstr++) {
14170 obj->SpatialInertia[b_kstr] = tmp_2[b_kstr];
14171 }
14172
14173 obj->JointInternal.InTree = false;
14174 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
14175 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_3[b_kstr];
14176 }
14177
14178 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
14179 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_3[b_kstr];
14180 }
14181
14182 b_kstr = obj->JointInternal.NameInternal->size[0] *
14183 obj->JointInternal.NameInternal->size[1];
14184 obj->JointInternal.NameInternal->size[0] = 1;
14185 obj->JointInternal.NameInternal->size[1] = 11;
14186 cartes_emxEnsureCapacity_char_T(obj->JointInternal.NameInternal, b_kstr);
14187 for (b_kstr = 0; b_kstr < 11; b_kstr++) {
14188 obj->JointInternal.NameInternal->data[b_kstr] = tmp_4[b_kstr];
14189 }
14190
14191 b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];
14192 obj->JointInternal.Type->size[0] = 1;
14193 obj->JointInternal.Type->size[1] = 8;
14194 cartes_emxEnsureCapacity_char_T(obj->JointInternal.Type, b_kstr);
14195 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
14196 obj->JointInternal.Type->data[b_kstr] = tmp_5[b_kstr];
14197 }
14198
14199 cartesian_trajec_emxInit_char_T(&switch_expression, 2);
14200 b_kstr = switch_expression->size[0] * switch_expression->size[1];
14201 switch_expression->size[0] = 1;
14202 switch_expression->size[1] = obj->JointInternal.Type->size[1];
14203 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
14204 loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]
14205 - 1;
14206 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
14207 switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];
14208 }
14209
14210 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
14211 b[b_kstr] = tmp_5[b_kstr];
14212 }
14213
14214 b_bool = false;
14215 if (switch_expression->size[1] == 8) {
14216 b_kstr = 1;
14217 do {
14218 exitg1 = 0;
14219 if (b_kstr - 1 < 8) {
14220 loop_ub = b_kstr - 1;
14221 if (switch_expression->data[loop_ub] != b[loop_ub]) {
14222 exitg1 = 1;
14223 } else {
14224 b_kstr++;
14225 }
14226 } else {
14227 b_bool = true;
14228 exitg1 = 1;
14229 }
14230 } while (exitg1 == 0);
14231 }
14232
14233 if (b_bool) {
14234 b_kstr = 0;
14235 } else {
14236 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
14237 b_0[b_kstr] = tmp_6[b_kstr];
14238 }
14239
14240 b_bool = false;
14241 if (switch_expression->size[1] == 9) {
14242 b_kstr = 1;
14243 do {
14244 exitg1 = 0;
14245 if (b_kstr - 1 < 9) {
14246 loop_ub = b_kstr - 1;
14247 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
14248 exitg1 = 1;
14249 } else {
14250 b_kstr++;
14251 }
14252 } else {
14253 b_bool = true;
14254 exitg1 = 1;
14255 }
14256 } while (exitg1 == 0);
14257 }
14258
14259 if (b_bool) {
14260 b_kstr = 1;
14261 } else {
14262 b_kstr = -1;
14263 }
14264 }
14265
14266 cartesian_trajec_emxFree_char_T(&switch_expression);
14267 switch (b_kstr) {
14268 case 0:
14269 tmp[0] = 0;
14270 tmp[1] = 0;
14271 tmp[2] = 1;
14272 tmp[3] = 0;
14273 tmp[4] = 0;
14274 tmp[5] = 0;
14275 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
14276 msubspace_data[b_kstr] = tmp[b_kstr];
14277 }
14278
14279 poslim_data[0] = -3.1415926535897931;
14280 poslim_data[1] = 3.1415926535897931;
14281 obj->JointInternal.VelocityNumber = 1.0;
14282 obj->JointInternal.PositionNumber = 1.0;
14283 obj->JointInternal.JointAxisInternal[0] = 0.0;
14284 obj->JointInternal.JointAxisInternal[1] = 0.0;
14285 obj->JointInternal.JointAxisInternal[2] = 1.0;
14286 break;
14287
14288 case 1:
14289 tmp[0] = 0;
14290 tmp[1] = 0;
14291 tmp[2] = 0;
14292 tmp[3] = 0;
14293 tmp[4] = 0;
14294 tmp[5] = 1;
14295 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
14296 msubspace_data[b_kstr] = tmp[b_kstr];
14297 }
14298
14299 poslim_data[0] = -0.5;
14300 poslim_data[1] = 0.5;
14301 obj->JointInternal.VelocityNumber = 1.0;
14302 obj->JointInternal.PositionNumber = 1.0;
14303 obj->JointInternal.JointAxisInternal[0] = 0.0;
14304 obj->JointInternal.JointAxisInternal[1] = 0.0;
14305 obj->JointInternal.JointAxisInternal[2] = 1.0;
14306 break;
14307
14308 default:
14309 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
14310 msubspace_data[b_kstr] = 0;
14311 }
14312
14313 poslim_data[0] = 0.0;
14314 poslim_data[1] = 0.0;
14315 obj->JointInternal.VelocityNumber = 0.0;
14316 obj->JointInternal.PositionNumber = 0.0;
14317 obj->JointInternal.JointAxisInternal[0] = 0.0;
14318 obj->JointInternal.JointAxisInternal[1] = 0.0;
14319 obj->JointInternal.JointAxisInternal[2] = 0.0;
14320 break;
14321 }
14322
14323 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
14324 obj->JointInternal.MotionSubspace->size[1];
14325 obj->JointInternal.MotionSubspace->size[0] = 6;
14326 obj->JointInternal.MotionSubspace->size[1] = 1;
14327 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
14328 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
14329 obj->JointInternal.MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
14330 }
14331
14332 b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *
14333 obj->JointInternal.PositionLimitsInternal->size[1];
14334 obj->JointInternal.PositionLimitsInternal->size[0] = 1;
14335 obj->JointInternal.PositionLimitsInternal->size[1] = 2;
14336 cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,
14337 b_kstr);
14338 for (b_kstr = 0; b_kstr < 2; b_kstr++) {
14339 obj->JointInternal.PositionLimitsInternal->data[b_kstr] = poslim_data[b_kstr];
14340 }
14341
14342 b_kstr = obj->JointInternal.HomePositionInternal->size[0];
14343 obj->JointInternal.HomePositionInternal->size[0] = 1;
14344 cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,
14345 b_kstr);
14346 for (b_kstr = 0; b_kstr < 1; b_kstr++) {
14347 obj->JointInternal.HomePositionInternal->data[0] = 0.0;
14348 }
14349
14350 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
14351 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_7[b_kstr];
14352 }
14353
14354 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
14355 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_8[b_kstr];
14356 }
14357
14358 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
14359 obj->JointInternal.MotionSubspace->size[1];
14360 obj->JointInternal.MotionSubspace->size[0] = 6;
14361 obj->JointInternal.MotionSubspace->size[1] = 1;
14362 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
14363 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
14364 obj->JointInternal.MotionSubspace->data[b_kstr] = tmp_9[b_kstr];
14365 }
14366
14367 obj->JointInternal.InTree = true;
14368 b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *
14369 obj->JointInternal.PositionLimitsInternal->size[1];
14370 obj->JointInternal.PositionLimitsInternal->size[0] = 1;
14371 obj->JointInternal.PositionLimitsInternal->size[1] = 2;
14372 cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,
14373 b_kstr);
14374 obj->JointInternal.PositionLimitsInternal->data[0] = -1.79768912955;
14375 obj->JointInternal.PositionLimitsInternal->data
14376 [obj->JointInternal.PositionLimitsInternal->size[0]] = 1.79768912955;
14377 obj->JointInternal.JointAxisInternal[0] = 0.0;
14378 obj->JointInternal.JointAxisInternal[1] = 1.0;
14379 obj->JointInternal.JointAxisInternal[2] = 0.0;
14380 b_kstr = obj->JointInternal.HomePositionInternal->size[0];
14381 obj->JointInternal.HomePositionInternal->size[0] = 1;
14382 cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,
14383 b_kstr);
14384 obj->JointInternal.HomePositionInternal->data[0] = 0.0;
14385 return b_obj;
14386}
14387
14388static v_robotics_manip_internal_Rig_T *RigidBody_Rigid_astwhqf2azttxab
14389 (v_robotics_manip_internal_Rig_T *obj)
14390{
14391 v_robotics_manip_internal_Rig_T *b_obj;
14392 int8_T msubspace_data[36];
14393 real_T poslim_data[12];
14394 emxArray_char_T_cartesian_tra_T *switch_expression;
14395 boolean_T b_bool;
14396 int32_T b_kstr;
14397 char_T b[8];
14398 char_T b_0[9];
14399 int32_T loop_ub;
14400 int8_T tmp[6];
14401 static const char_T tmp_0[10] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
14402 '6' };
14403
14404 static const real_T tmp_1[9] = { 1.0067862401982823E-5, 2.0153545938371486E-9,
14405 5.3285284099072352E-9, 2.0153545938371486E-9, 1.4574493611914028E-5,
14406 1.6742291194075022E-9, 5.3285284099072352E-9, 1.6742291194075022E-9,
14407 1.006193323459457E-5 };
14408
14409 static const real_T tmp_2[36] = { 1.0067862401982823E-5, 2.0153545938371486E-9,
14410 5.3285284099072352E-9, 0.0, -1.6782149839359722E-7, -0.00026087851925284686,
14411 2.0153545938371486E-9, 1.4574493611914028E-5, 1.6742291194075022E-9,
14412 1.6782149839359722E-7, 0.0, -1.957917481258634E-7, 5.3285284099072352E-9,
14413 1.6742291194075022E-9, 1.006193323459457E-5, 0.00026087851925284686,
14414 1.957917481258634E-7, 0.0, 0.0, 1.6782149839359722E-7,
14415 0.00026087851925284686, 0.0279702497322662, 0.0, 0.0, -1.6782149839359722E-7,
14416 0.0, 1.957917481258634E-7, 0.0, 0.0279702497322662, 0.0,
14417 -0.00026087851925284686, -1.957917481258634E-7, 0.0, 0.0, 0.0,
14418 0.0279702497322662 };
14419
14420 static const int8_T tmp_3[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,
14421 1 };
14422
14423 static const char_T tmp_4[11] = { 'e', 'd', 'o', '_', 'j', 'o', 'i', 'n', 't',
14424 '_', '6' };
14425
14426 static const char_T tmp_5[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
14427
14428 static const char_T tmp_6[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
14429
14430 static const real_T tmp_7[16] = { 1.0, 0.0, -0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
14431 0.0, 1.0, 0.0, 0.0, 0.0, 0.1745, 1.0 };
14432
14433 static const real_T tmp_8[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
14434 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
14435
14436 static const real_T tmp_9[36] = { 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
14437 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
14438 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
14439
14440 int32_T exitg1;
14441 b_obj = obj;
14442 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
14443 obj->NameInternal->size[0] = 1;
14444 obj->NameInternal->size[1] = 10;
14445 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);
14446 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
14447 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
14448 }
14449
14450 obj->ParentIndex = 6.0;
14451 obj->MassInternal = 0.0279702497322662;
14452 obj->CenterOfMassInternal[0] = 7.0E-6;
14453 obj->CenterOfMassInternal[1] = -0.009327;
14454 obj->CenterOfMassInternal[2] = 6.0E-6;
14455 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
14456 obj->InertiaInternal[b_kstr] = tmp_1[b_kstr];
14457 }
14458
14459 for (b_kstr = 0; b_kstr < 36; b_kstr++) {
14460 obj->SpatialInertia[b_kstr] = tmp_2[b_kstr];
14461 }
14462
14463 obj->JointInternal.InTree = false;
14464 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
14465 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_3[b_kstr];
14466 }
14467
14468 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
14469 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_3[b_kstr];
14470 }
14471
14472 b_kstr = obj->JointInternal.NameInternal->size[0] *
14473 obj->JointInternal.NameInternal->size[1];
14474 obj->JointInternal.NameInternal->size[0] = 1;
14475 obj->JointInternal.NameInternal->size[1] = 11;
14476 cartes_emxEnsureCapacity_char_T(obj->JointInternal.NameInternal, b_kstr);
14477 for (b_kstr = 0; b_kstr < 11; b_kstr++) {
14478 obj->JointInternal.NameInternal->data[b_kstr] = tmp_4[b_kstr];
14479 }
14480
14481 b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];
14482 obj->JointInternal.Type->size[0] = 1;
14483 obj->JointInternal.Type->size[1] = 8;
14484 cartes_emxEnsureCapacity_char_T(obj->JointInternal.Type, b_kstr);
14485 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
14486 obj->JointInternal.Type->data[b_kstr] = tmp_5[b_kstr];
14487 }
14488
14489 cartesian_trajec_emxInit_char_T(&switch_expression, 2);
14490 b_kstr = switch_expression->size[0] * switch_expression->size[1];
14491 switch_expression->size[0] = 1;
14492 switch_expression->size[1] = obj->JointInternal.Type->size[1];
14493 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
14494 loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]
14495 - 1;
14496 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
14497 switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];
14498 }
14499
14500 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
14501 b[b_kstr] = tmp_5[b_kstr];
14502 }
14503
14504 b_bool = false;
14505 if (switch_expression->size[1] == 8) {
14506 b_kstr = 1;
14507 do {
14508 exitg1 = 0;
14509 if (b_kstr - 1 < 8) {
14510 loop_ub = b_kstr - 1;
14511 if (switch_expression->data[loop_ub] != b[loop_ub]) {
14512 exitg1 = 1;
14513 } else {
14514 b_kstr++;
14515 }
14516 } else {
14517 b_bool = true;
14518 exitg1 = 1;
14519 }
14520 } while (exitg1 == 0);
14521 }
14522
14523 if (b_bool) {
14524 b_kstr = 0;
14525 } else {
14526 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
14527 b_0[b_kstr] = tmp_6[b_kstr];
14528 }
14529
14530 b_bool = false;
14531 if (switch_expression->size[1] == 9) {
14532 b_kstr = 1;
14533 do {
14534 exitg1 = 0;
14535 if (b_kstr - 1 < 9) {
14536 loop_ub = b_kstr - 1;
14537 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
14538 exitg1 = 1;
14539 } else {
14540 b_kstr++;
14541 }
14542 } else {
14543 b_bool = true;
14544 exitg1 = 1;
14545 }
14546 } while (exitg1 == 0);
14547 }
14548
14549 if (b_bool) {
14550 b_kstr = 1;
14551 } else {
14552 b_kstr = -1;
14553 }
14554 }
14555
14556 cartesian_trajec_emxFree_char_T(&switch_expression);
14557 switch (b_kstr) {
14558 case 0:
14559 tmp[0] = 0;
14560 tmp[1] = 0;
14561 tmp[2] = 1;
14562 tmp[3] = 0;
14563 tmp[4] = 0;
14564 tmp[5] = 0;
14565 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
14566 msubspace_data[b_kstr] = tmp[b_kstr];
14567 }
14568
14569 poslim_data[0] = -3.1415926535897931;
14570 poslim_data[1] = 3.1415926535897931;
14571 obj->JointInternal.VelocityNumber = 1.0;
14572 obj->JointInternal.PositionNumber = 1.0;
14573 obj->JointInternal.JointAxisInternal[0] = 0.0;
14574 obj->JointInternal.JointAxisInternal[1] = 0.0;
14575 obj->JointInternal.JointAxisInternal[2] = 1.0;
14576 break;
14577
14578 case 1:
14579 tmp[0] = 0;
14580 tmp[1] = 0;
14581 tmp[2] = 0;
14582 tmp[3] = 0;
14583 tmp[4] = 0;
14584 tmp[5] = 1;
14585 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
14586 msubspace_data[b_kstr] = tmp[b_kstr];
14587 }
14588
14589 poslim_data[0] = -0.5;
14590 poslim_data[1] = 0.5;
14591 obj->JointInternal.VelocityNumber = 1.0;
14592 obj->JointInternal.PositionNumber = 1.0;
14593 obj->JointInternal.JointAxisInternal[0] = 0.0;
14594 obj->JointInternal.JointAxisInternal[1] = 0.0;
14595 obj->JointInternal.JointAxisInternal[2] = 1.0;
14596 break;
14597
14598 default:
14599 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
14600 msubspace_data[b_kstr] = 0;
14601 }
14602
14603 poslim_data[0] = 0.0;
14604 poslim_data[1] = 0.0;
14605 obj->JointInternal.VelocityNumber = 0.0;
14606 obj->JointInternal.PositionNumber = 0.0;
14607 obj->JointInternal.JointAxisInternal[0] = 0.0;
14608 obj->JointInternal.JointAxisInternal[1] = 0.0;
14609 obj->JointInternal.JointAxisInternal[2] = 0.0;
14610 break;
14611 }
14612
14613 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
14614 obj->JointInternal.MotionSubspace->size[1];
14615 obj->JointInternal.MotionSubspace->size[0] = 6;
14616 obj->JointInternal.MotionSubspace->size[1] = 1;
14617 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
14618 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
14619 obj->JointInternal.MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
14620 }
14621
14622 b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *
14623 obj->JointInternal.PositionLimitsInternal->size[1];
14624 obj->JointInternal.PositionLimitsInternal->size[0] = 1;
14625 obj->JointInternal.PositionLimitsInternal->size[1] = 2;
14626 cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,
14627 b_kstr);
14628 for (b_kstr = 0; b_kstr < 2; b_kstr++) {
14629 obj->JointInternal.PositionLimitsInternal->data[b_kstr] = poslim_data[b_kstr];
14630 }
14631
14632 b_kstr = obj->JointInternal.HomePositionInternal->size[0];
14633 obj->JointInternal.HomePositionInternal->size[0] = 1;
14634 cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,
14635 b_kstr);
14636 for (b_kstr = 0; b_kstr < 1; b_kstr++) {
14637 obj->JointInternal.HomePositionInternal->data[0] = 0.0;
14638 }
14639
14640 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
14641 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_7[b_kstr];
14642 }
14643
14644 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
14645 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_8[b_kstr];
14646 }
14647
14648 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
14649 obj->JointInternal.MotionSubspace->size[1];
14650 obj->JointInternal.MotionSubspace->size[0] = 6;
14651 obj->JointInternal.MotionSubspace->size[1] = 1;
14652 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
14653 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
14654 obj->JointInternal.MotionSubspace->data[b_kstr] = tmp_9[b_kstr];
14655 }
14656
14657 obj->JointInternal.InTree = true;
14658 b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *
14659 obj->JointInternal.PositionLimitsInternal->size[1];
14660 obj->JointInternal.PositionLimitsInternal->size[0] = 1;
14661 obj->JointInternal.PositionLimitsInternal->size[1] = 2;
14662 cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,
14663 b_kstr);
14664 obj->JointInternal.PositionLimitsInternal->data[0] = -4.71238898038;
14665 obj->JointInternal.PositionLimitsInternal->data
14666 [obj->JointInternal.PositionLimitsInternal->size[0]] = 4.71238898038;
14667 obj->JointInternal.JointAxisInternal[0] = 0.0;
14668 obj->JointInternal.JointAxisInternal[1] = 0.0;
14669 obj->JointInternal.JointAxisInternal[2] = 1.0;
14670 b_kstr = obj->JointInternal.HomePositionInternal->size[0];
14671 obj->JointInternal.HomePositionInternal->size[0] = 1;
14672 cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,
14673 b_kstr);
14674 obj->JointInternal.HomePositionInternal->data[0] = 0.0;
14675 return b_obj;
14676}
14677
14678static y_robotics_manip_internal_Rig_T *c_RigidBodyTree_RigidBodyTree_a
14679 (y_robotics_manip_internal_Rig_T *obj, v_robotics_manip_internal_Rig_T *iobj_0,
14680 v_robotics_manip_internal_Rig_T *iobj_1, v_robotics_manip_internal_Rig_T
14681 *iobj_2, v_robotics_manip_internal_Rig_T *iobj_3,
14682 v_robotics_manip_internal_Rig_T *iobj_4, v_robotics_manip_internal_Rig_T
14683 *iobj_5, v_robotics_manip_internal_Rig_T *iobj_6,
14684 v_robotics_manip_internal_Rig_T *iobj_7)
14685{
14686 y_robotics_manip_internal_Rig_T *b_obj;
14687 v_robotics_manip_internal_Rig_T *obj_0;
14688 int8_T msubspace_data[36];
14689 real_T poslim_data[12];
14690 int8_T b_I[9];
14691 emxArray_char_T_cartesian_tra_T *switch_expression;
14692 boolean_T b_bool;
14693 int32_T b_kstr;
14694 char_T b[8];
14695 char_T b_0[9];
14696 int32_T loop_ub;
14697 int8_T tmp[6];
14698 static const char_T tmp_0[11] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
14699 'e', 'e' };
14700
14701 static const real_T tmp_1[36] = { 0.0, 0.0, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0, 0.0,
14702 0.0, 0.0, -0.0, 0.0, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0,
14703 -0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0, 0.0 };
14704
14705 static const int8_T tmp_2[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,
14706 1 };
14707
14708 static const char_T tmp_3[12] = { 'e', 'd', 'o', '_', 'j', 'o', 'i', 'n', 't',
14709 '_', 'e', 'e' };
14710
14711 static const char_T tmp_4[5] = { 'f', 'i', 'x', 'e', 'd' };
14712
14713 static const char_T tmp_5[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
14714
14715 static const char_T tmp_6[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
14716
14717 static const real_T tmp_7[16] = { 1.0, 0.0, -0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
14718 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
14719
14720 static const real_T tmp_8[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
14721 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
14722
14723 static const char_T tmp_9[5] = { 'w', 'o', 'r', 'l', 'd' };
14724
14725 static const char_T tmp_a[9] = { 'w', 'o', 'r', 'l', 'd', '_', 'j', 'n', 't' };
14726
14727 int32_T exitg1;
14728 b_obj = obj;
14729 obj->Bodies[0] = c_RigidBody_RigidBody_astwhqf2a(iobj_7);
14730 obj->Bodies[0]->Index = 1.0;
14731 obj->Bodies[1] = RigidBody_RigidBody_astwhqf2az(iobj_0);
14732 obj->Bodies[1]->Index = 2.0;
14733 obj->Bodies[2] = RigidBody_RigidBody_astwhqf2azt(iobj_1);
14734 obj->Bodies[2]->Index = 3.0;
14735 obj->Bodies[3] = RigidBody_RigidBod_astwhqf2aztt(iobj_2);
14736 obj->Bodies[3]->Index = 4.0;
14737 obj->Bodies[4] = RigidBody_RigidBo_astwhqf2azttx(iobj_3);
14738 obj->Bodies[4]->Index = 5.0;
14739 obj->Bodies[5] = RigidBody_RigidB_astwhqf2azttxa(iobj_4);
14740 obj->Bodies[5]->Index = 6.0;
14741 obj->Bodies[6] = RigidBody_Rigid_astwhqf2azttxab(iobj_5);
14742 obj->Bodies[6]->Index = 7.0;
14743 b_kstr = iobj_6->NameInternal->size[0] * iobj_6->NameInternal->size[1];
14744 iobj_6->NameInternal->size[0] = 1;
14745 iobj_6->NameInternal->size[1] = 11;
14746 cartes_emxEnsureCapacity_char_T(iobj_6->NameInternal, b_kstr);
14747 for (b_kstr = 0; b_kstr < 11; b_kstr++) {
14748 iobj_6->NameInternal->data[b_kstr] = tmp_0[b_kstr];
14749 }
14750
14751 iobj_6->ParentIndex = 7.0;
14752 iobj_6->MassInternal = 0.0;
14753 iobj_6->CenterOfMassInternal[0] = 0.0;
14754 iobj_6->CenterOfMassInternal[1] = 0.0;
14755 iobj_6->CenterOfMassInternal[2] = 0.0;
14756 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
14757 iobj_6->InertiaInternal[b_kstr] = 0.0;
14758 }
14759
14760 for (b_kstr = 0; b_kstr < 36; b_kstr++) {
14761 iobj_6->SpatialInertia[b_kstr] = tmp_1[b_kstr];
14762 }
14763
14764 iobj_6->JointInternal.InTree = false;
14765 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
14766 iobj_6->JointInternal.JointToParentTransform[b_kstr] = tmp_2[b_kstr];
14767 }
14768
14769 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
14770 iobj_6->JointInternal.ChildToJointTransform[b_kstr] = tmp_2[b_kstr];
14771 }
14772
14773 b_kstr = iobj_6->JointInternal.NameInternal->size[0] *
14774 iobj_6->JointInternal.NameInternal->size[1];
14775 iobj_6->JointInternal.NameInternal->size[0] = 1;
14776 iobj_6->JointInternal.NameInternal->size[1] = 12;
14777 cartes_emxEnsureCapacity_char_T(iobj_6->JointInternal.NameInternal, b_kstr);
14778 for (b_kstr = 0; b_kstr < 12; b_kstr++) {
14779 iobj_6->JointInternal.NameInternal->data[b_kstr] = tmp_3[b_kstr];
14780 }
14781
14782 b_kstr = iobj_6->JointInternal.Type->size[0] * iobj_6->
14783 JointInternal.Type->size[1];
14784 iobj_6->JointInternal.Type->size[0] = 1;
14785 iobj_6->JointInternal.Type->size[1] = 5;
14786 cartes_emxEnsureCapacity_char_T(iobj_6->JointInternal.Type, b_kstr);
14787 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
14788 iobj_6->JointInternal.Type->data[b_kstr] = tmp_4[b_kstr];
14789 }
14790
14791 cartesian_trajec_emxInit_char_T(&switch_expression, 2);
14792 b_kstr = switch_expression->size[0] * switch_expression->size[1];
14793 switch_expression->size[0] = 1;
14794 switch_expression->size[1] = iobj_6->JointInternal.Type->size[1];
14795 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
14796 loop_ub = iobj_6->JointInternal.Type->size[0] * iobj_6->
14797 JointInternal.Type->size[1] - 1;
14798 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
14799 switch_expression->data[b_kstr] = iobj_6->JointInternal.Type->data[b_kstr];
14800 }
14801
14802 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
14803 b[b_kstr] = tmp_5[b_kstr];
14804 }
14805
14806 b_bool = false;
14807 if (switch_expression->size[1] == 8) {
14808 b_kstr = 1;
14809 do {
14810 exitg1 = 0;
14811 if (b_kstr - 1 < 8) {
14812 loop_ub = b_kstr - 1;
14813 if (switch_expression->data[loop_ub] != b[loop_ub]) {
14814 exitg1 = 1;
14815 } else {
14816 b_kstr++;
14817 }
14818 } else {
14819 b_bool = true;
14820 exitg1 = 1;
14821 }
14822 } while (exitg1 == 0);
14823 }
14824
14825 if (b_bool) {
14826 b_kstr = 0;
14827 } else {
14828 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
14829 b_0[b_kstr] = tmp_6[b_kstr];
14830 }
14831
14832 b_bool = false;
14833 if (switch_expression->size[1] == 9) {
14834 b_kstr = 1;
14835 do {
14836 exitg1 = 0;
14837 if (b_kstr - 1 < 9) {
14838 loop_ub = b_kstr - 1;
14839 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
14840 exitg1 = 1;
14841 } else {
14842 b_kstr++;
14843 }
14844 } else {
14845 b_bool = true;
14846 exitg1 = 1;
14847 }
14848 } while (exitg1 == 0);
14849 }
14850
14851 if (b_bool) {
14852 b_kstr = 1;
14853 } else {
14854 b_kstr = -1;
14855 }
14856 }
14857
14858 switch (b_kstr) {
14859 case 0:
14860 tmp[0] = 0;
14861 tmp[1] = 0;
14862 tmp[2] = 1;
14863 tmp[3] = 0;
14864 tmp[4] = 0;
14865 tmp[5] = 0;
14866 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
14867 msubspace_data[b_kstr] = tmp[b_kstr];
14868 }
14869
14870 poslim_data[0] = -3.1415926535897931;
14871 poslim_data[1] = 3.1415926535897931;
14872 iobj_6->JointInternal.VelocityNumber = 1.0;
14873 iobj_6->JointInternal.PositionNumber = 1.0;
14874 iobj_6->JointInternal.JointAxisInternal[0] = 0.0;
14875 iobj_6->JointInternal.JointAxisInternal[1] = 0.0;
14876 iobj_6->JointInternal.JointAxisInternal[2] = 1.0;
14877 break;
14878
14879 case 1:
14880 tmp[0] = 0;
14881 tmp[1] = 0;
14882 tmp[2] = 0;
14883 tmp[3] = 0;
14884 tmp[4] = 0;
14885 tmp[5] = 1;
14886 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
14887 msubspace_data[b_kstr] = tmp[b_kstr];
14888 }
14889
14890 poslim_data[0] = -0.5;
14891 poslim_data[1] = 0.5;
14892 iobj_6->JointInternal.VelocityNumber = 1.0;
14893 iobj_6->JointInternal.PositionNumber = 1.0;
14894 iobj_6->JointInternal.JointAxisInternal[0] = 0.0;
14895 iobj_6->JointInternal.JointAxisInternal[1] = 0.0;
14896 iobj_6->JointInternal.JointAxisInternal[2] = 1.0;
14897 break;
14898
14899 default:
14900 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
14901 msubspace_data[b_kstr] = 0;
14902 }
14903
14904 poslim_data[0] = 0.0;
14905 poslim_data[1] = 0.0;
14906 iobj_6->JointInternal.VelocityNumber = 0.0;
14907 iobj_6->JointInternal.PositionNumber = 0.0;
14908 iobj_6->JointInternal.JointAxisInternal[0] = 0.0;
14909 iobj_6->JointInternal.JointAxisInternal[1] = 0.0;
14910 iobj_6->JointInternal.JointAxisInternal[2] = 0.0;
14911 break;
14912 }
14913
14914 b_kstr = iobj_6->JointInternal.MotionSubspace->size[0] *
14915 iobj_6->JointInternal.MotionSubspace->size[1];
14916 iobj_6->JointInternal.MotionSubspace->size[0] = 6;
14917 iobj_6->JointInternal.MotionSubspace->size[1] = 1;
14918 cartes_emxEnsureCapacity_real_T(iobj_6->JointInternal.MotionSubspace, b_kstr);
14919 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
14920 iobj_6->JointInternal.MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
14921 }
14922
14923 b_kstr = iobj_6->JointInternal.PositionLimitsInternal->size[0] *
14924 iobj_6->JointInternal.PositionLimitsInternal->size[1];
14925 iobj_6->JointInternal.PositionLimitsInternal->size[0] = 1;
14926 iobj_6->JointInternal.PositionLimitsInternal->size[1] = 2;
14927 cartes_emxEnsureCapacity_real_T(iobj_6->JointInternal.PositionLimitsInternal,
14928 b_kstr);
14929 for (b_kstr = 0; b_kstr < 2; b_kstr++) {
14930 iobj_6->JointInternal.PositionLimitsInternal->data[b_kstr] =
14931 poslim_data[b_kstr];
14932 }
14933
14934 b_kstr = iobj_6->JointInternal.HomePositionInternal->size[0];
14935 iobj_6->JointInternal.HomePositionInternal->size[0] = 1;
14936 cartes_emxEnsureCapacity_real_T(iobj_6->JointInternal.HomePositionInternal,
14937 b_kstr);
14938 for (b_kstr = 0; b_kstr < 1; b_kstr++) {
14939 iobj_6->JointInternal.HomePositionInternal->data[0] = 0.0;
14940 }
14941
14942 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
14943 iobj_6->JointInternal.JointToParentTransform[b_kstr] = tmp_7[b_kstr];
14944 }
14945
14946 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
14947 iobj_6->JointInternal.ChildToJointTransform[b_kstr] = tmp_8[b_kstr];
14948 }
14949
14950 b_kstr = iobj_6->JointInternal.MotionSubspace->size[0] *
14951 iobj_6->JointInternal.MotionSubspace->size[1];
14952 iobj_6->JointInternal.MotionSubspace->size[0] = 6;
14953 iobj_6->JointInternal.MotionSubspace->size[1] = 1;
14954 cartes_emxEnsureCapacity_real_T(iobj_6->JointInternal.MotionSubspace, b_kstr);
14955 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
14956 iobj_6->JointInternal.MotionSubspace->data[b_kstr] = 0.0;
14957 }
14958
14959 iobj_6->JointInternal.InTree = true;
14960 b_kstr = iobj_6->JointInternal.PositionLimitsInternal->size[0] *
14961 iobj_6->JointInternal.PositionLimitsInternal->size[1];
14962 iobj_6->JointInternal.PositionLimitsInternal->size[0] = 1;
14963 iobj_6->JointInternal.PositionLimitsInternal->size[1] = 2;
14964 cartes_emxEnsureCapacity_real_T(iobj_6->JointInternal.PositionLimitsInternal,
14965 b_kstr);
14966 iobj_6->JointInternal.PositionLimitsInternal->data[0] = 0.0;
14967 iobj_6->JointInternal.PositionLimitsInternal->data
14968 [iobj_6->JointInternal.PositionLimitsInternal->size[0]] = 0.0;
14969 iobj_6->JointInternal.JointAxisInternal[0] = 0.0;
14970 iobj_6->JointInternal.JointAxisInternal[1] = 0.0;
14971 iobj_6->JointInternal.JointAxisInternal[2] = 0.0;
14972 b_kstr = iobj_6->JointInternal.HomePositionInternal->size[0];
14973 iobj_6->JointInternal.HomePositionInternal->size[0] = 1;
14974 cartes_emxEnsureCapacity_real_T(iobj_6->JointInternal.HomePositionInternal,
14975 b_kstr);
14976 iobj_6->JointInternal.HomePositionInternal->data[0] = 0.0;
14977 obj->Bodies[7] = iobj_6;
14978 obj->Bodies[7]->Index = 8.0;
14979 obj->NumBodies = 8.0;
14980 obj->Gravity[0] = 0.0;
14981 obj->Gravity[1] = 0.0;
14982 obj->Gravity[2] = 0.0;
14983 obj_0 = &obj->Base;
14984 b_kstr = obj->Base.NameInternal->size[0] * obj->Base.NameInternal->size[1];
14985 obj->Base.NameInternal->size[0] = 1;
14986 obj->Base.NameInternal->size[1] = 5;
14987 cartes_emxEnsureCapacity_char_T(obj->Base.NameInternal, b_kstr);
14988 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
14989 obj->Base.NameInternal->data[b_kstr] = tmp_9[b_kstr];
14990 }
14991
14992 obj->Base.JointInternal.InTree = false;
14993 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
14994 obj_0->JointInternal.JointToParentTransform[b_kstr] = tmp_2[b_kstr];
14995 }
14996
14997 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
14998 obj_0->JointInternal.ChildToJointTransform[b_kstr] = tmp_2[b_kstr];
14999 }
15000
15001 b_kstr = obj->Base.JointInternal.NameInternal->size[0] *
15002 obj->Base.JointInternal.NameInternal->size[1];
15003 obj->Base.JointInternal.NameInternal->size[0] = 1;
15004 obj->Base.JointInternal.NameInternal->size[1] = 9;
15005 cartes_emxEnsureCapacity_char_T(obj->Base.JointInternal.NameInternal, b_kstr);
15006 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
15007 obj_0->JointInternal.NameInternal->data[b_kstr] = tmp_a[b_kstr];
15008 }
15009
15010 b_kstr = obj->Base.JointInternal.Type->size[0] * obj->
15011 Base.JointInternal.Type->size[1];
15012 obj->Base.JointInternal.Type->size[0] = 1;
15013 obj->Base.JointInternal.Type->size[1] = 5;
15014 cartes_emxEnsureCapacity_char_T(obj->Base.JointInternal.Type, b_kstr);
15015 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
15016 obj_0->JointInternal.Type->data[b_kstr] = tmp_4[b_kstr];
15017 }
15018
15019 b_kstr = switch_expression->size[0] * switch_expression->size[1];
15020 switch_expression->size[0] = 1;
15021 switch_expression->size[1] = obj->Base.JointInternal.Type->size[1];
15022 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
15023 loop_ub = obj->Base.JointInternal.Type->size[0] * obj->
15024 Base.JointInternal.Type->size[1] - 1;
15025 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
15026 switch_expression->data[b_kstr] = obj_0->JointInternal.Type->data[b_kstr];
15027 }
15028
15029 b_bool = false;
15030 if (switch_expression->size[1] == 8) {
15031 b_kstr = 1;
15032 do {
15033 exitg1 = 0;
15034 if (b_kstr - 1 < 8) {
15035 loop_ub = b_kstr - 1;
15036 if (switch_expression->data[loop_ub] != b[loop_ub]) {
15037 exitg1 = 1;
15038 } else {
15039 b_kstr++;
15040 }
15041 } else {
15042 b_bool = true;
15043 exitg1 = 1;
15044 }
15045 } while (exitg1 == 0);
15046 }
15047
15048 if (b_bool) {
15049 b_kstr = 0;
15050 } else {
15051 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
15052 b_0[b_kstr] = tmp_6[b_kstr];
15053 }
15054
15055 b_bool = false;
15056 if (switch_expression->size[1] == 9) {
15057 b_kstr = 1;
15058 do {
15059 exitg1 = 0;
15060 if (b_kstr - 1 < 9) {
15061 loop_ub = b_kstr - 1;
15062 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
15063 exitg1 = 1;
15064 } else {
15065 b_kstr++;
15066 }
15067 } else {
15068 b_bool = true;
15069 exitg1 = 1;
15070 }
15071 } while (exitg1 == 0);
15072 }
15073
15074 if (b_bool) {
15075 b_kstr = 1;
15076 } else {
15077 b_kstr = -1;
15078 }
15079 }
15080
15081 cartesian_trajec_emxFree_char_T(&switch_expression);
15082 switch (b_kstr) {
15083 case 0:
15084 tmp[0] = 0;
15085 tmp[1] = 0;
15086 tmp[2] = 1;
15087 tmp[3] = 0;
15088 tmp[4] = 0;
15089 tmp[5] = 0;
15090 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
15091 msubspace_data[b_kstr] = tmp[b_kstr];
15092 }
15093
15094 poslim_data[0] = -3.1415926535897931;
15095 poslim_data[1] = 3.1415926535897931;
15096 obj->Base.JointInternal.VelocityNumber = 1.0;
15097 obj->Base.JointInternal.PositionNumber = 1.0;
15098 obj->Base.JointInternal.JointAxisInternal[0] = 0.0;
15099 obj->Base.JointInternal.JointAxisInternal[1] = 0.0;
15100 obj->Base.JointInternal.JointAxisInternal[2] = 1.0;
15101 break;
15102
15103 case 1:
15104 tmp[0] = 0;
15105 tmp[1] = 0;
15106 tmp[2] = 0;
15107 tmp[3] = 0;
15108 tmp[4] = 0;
15109 tmp[5] = 1;
15110 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
15111 msubspace_data[b_kstr] = tmp[b_kstr];
15112 }
15113
15114 poslim_data[0] = -0.5;
15115 poslim_data[1] = 0.5;
15116 obj->Base.JointInternal.VelocityNumber = 1.0;
15117 obj->Base.JointInternal.PositionNumber = 1.0;
15118 obj->Base.JointInternal.JointAxisInternal[0] = 0.0;
15119 obj->Base.JointInternal.JointAxisInternal[1] = 0.0;
15120 obj->Base.JointInternal.JointAxisInternal[2] = 1.0;
15121 break;
15122
15123 default:
15124 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
15125 msubspace_data[b_kstr] = 0;
15126 }
15127
15128 poslim_data[0] = 0.0;
15129 poslim_data[1] = 0.0;
15130 obj->Base.JointInternal.VelocityNumber = 0.0;
15131 obj->Base.JointInternal.PositionNumber = 0.0;
15132 obj->Base.JointInternal.JointAxisInternal[0] = 0.0;
15133 obj->Base.JointInternal.JointAxisInternal[1] = 0.0;
15134 obj->Base.JointInternal.JointAxisInternal[2] = 0.0;
15135 break;
15136 }
15137
15138 b_kstr = obj->Base.JointInternal.MotionSubspace->size[0] *
15139 obj->Base.JointInternal.MotionSubspace->size[1];
15140 obj->Base.JointInternal.MotionSubspace->size[0] = 6;
15141 obj->Base.JointInternal.MotionSubspace->size[1] = 1;
15142 cartes_emxEnsureCapacity_real_T(obj->Base.JointInternal.MotionSubspace, b_kstr);
15143 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
15144 obj_0->JointInternal.MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
15145 }
15146
15147 b_kstr = obj->Base.JointInternal.PositionLimitsInternal->size[0] *
15148 obj->Base.JointInternal.PositionLimitsInternal->size[1];
15149 obj->Base.JointInternal.PositionLimitsInternal->size[0] = 1;
15150 obj->Base.JointInternal.PositionLimitsInternal->size[1] = 2;
15151 cartes_emxEnsureCapacity_real_T(obj->Base.JointInternal.PositionLimitsInternal,
15152 b_kstr);
15153 for (b_kstr = 0; b_kstr < 2; b_kstr++) {
15154 obj_0->JointInternal.PositionLimitsInternal->data[b_kstr] =
15155 poslim_data[b_kstr];
15156 }
15157
15158 b_kstr = obj->Base.JointInternal.HomePositionInternal->size[0];
15159 obj->Base.JointInternal.HomePositionInternal->size[0] = 1;
15160 cartes_emxEnsureCapacity_real_T(obj->Base.JointInternal.HomePositionInternal,
15161 b_kstr);
15162 for (b_kstr = 0; b_kstr < 1; b_kstr++) {
15163 obj_0->JointInternal.HomePositionInternal->data[0] = 0.0;
15164 }
15165
15166 obj->Base.Index = -1.0;
15167 obj->Base.ParentIndex = -1.0;
15168 obj->Base.MassInternal = 1.0;
15169 obj->Base.CenterOfMassInternal[0] = 0.0;
15170 obj->Base.CenterOfMassInternal[1] = 0.0;
15171 obj->Base.CenterOfMassInternal[2] = 0.0;
15172 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
15173 b_I[b_kstr] = 0;
15174 }
15175
15176 b_I[0] = 1;
15177 b_I[4] = 1;
15178 b_I[8] = 1;
15179 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
15180 obj->Base.InertiaInternal[b_kstr] = b_I[b_kstr];
15181 }
15182
15183 for (b_kstr = 0; b_kstr < 36; b_kstr++) {
15184 msubspace_data[b_kstr] = 0;
15185 }
15186
15187 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
15188 msubspace_data[b_kstr + 6 * b_kstr] = 1;
15189 }
15190
15191 for (b_kstr = 0; b_kstr < 36; b_kstr++) {
15192 obj->Base.SpatialInertia[b_kstr] = msubspace_data[b_kstr];
15193 }
15194
15195 return b_obj;
15196}
15197
15198static void cartesian_trajectory_plann_rand(real_T r[5])
15199{
15200 for (cartesian_trajectory_planner_B.b_k_a = 0;
15201 cartesian_trajectory_planner_B.b_k_a < 5;
15202 cartesian_trajectory_planner_B.b_k_a++) {
15203 memcpy(&cartesian_trajectory_planner_B.uv[0],
15204 &cartesian_trajectory_planner_DW.state_b[0], 625U * sizeof(uint32_T));
15205 cartesian__eml_rand_mt19937ar_a(cartesian_trajectory_planner_B.uv,
15206 cartesian_trajectory_planner_DW.state_b,
15207 &r[cartesian_trajectory_planner_B.b_k_a]);
15208 }
15209}
15210
15211static w_robotics_manip_internal_Rig_T *c_RigidBody_Rigid_b
15212 (w_robotics_manip_internal_Rig_T *obj, c_rigidBodyJoint_cartesian__a_T *iobj_0)
15213{
15214 w_robotics_manip_internal_Rig_T *b_obj;
15215 int8_T msubspace_data[36];
15216 real_T poslim_data[12];
15217 emxArray_char_T_cartesian_tra_T *switch_expression;
15218 boolean_T b_bool;
15219 int32_T b_kstr;
15220 char_T b[8];
15221 char_T b_0[9];
15222 int32_T loop_ub;
15223 int8_T tmp[6];
15224 static const char_T tmp_0[10] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
15225 '\x01' };
15226
15227 static const int8_T tmp_1[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,
15228 1 };
15229
15230 static const char_T tmp_2[14] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
15231 '\x01', '_', 'j', 'n', 't' };
15232
15233 static const char_T tmp_3[5] = { 'f', 'i', 'x', 'e', 'd' };
15234
15235 static const char_T tmp_4[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
15236
15237 static const char_T tmp_5[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
15238
15239 int32_T exitg1;
15240 b_obj = obj;
15241 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
15242 obj->NameInternal->size[0] = 1;
15243 obj->NameInternal->size[1] = 10;
15244 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);
15245 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
15246 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
15247 }
15248
15249 iobj_0->InTree = false;
15250 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
15251 iobj_0->JointToParentTransform[b_kstr] = tmp_1[b_kstr];
15252 }
15253
15254 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
15255 iobj_0->ChildToJointTransform[b_kstr] = tmp_1[b_kstr];
15256 }
15257
15258 b_kstr = iobj_0->NameInternal->size[0] * iobj_0->NameInternal->size[1];
15259 iobj_0->NameInternal->size[0] = 1;
15260 iobj_0->NameInternal->size[1] = 14;
15261 cartes_emxEnsureCapacity_char_T(iobj_0->NameInternal, b_kstr);
15262 for (b_kstr = 0; b_kstr < 14; b_kstr++) {
15263 iobj_0->NameInternal->data[b_kstr] = tmp_2[b_kstr];
15264 }
15265
15266 b_kstr = iobj_0->Type->size[0] * iobj_0->Type->size[1];
15267 iobj_0->Type->size[0] = 1;
15268 iobj_0->Type->size[1] = 5;
15269 cartes_emxEnsureCapacity_char_T(iobj_0->Type, b_kstr);
15270 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
15271 iobj_0->Type->data[b_kstr] = tmp_3[b_kstr];
15272 }
15273
15274 cartesian_trajec_emxInit_char_T(&switch_expression, 2);
15275 b_kstr = switch_expression->size[0] * switch_expression->size[1];
15276 switch_expression->size[0] = 1;
15277 switch_expression->size[1] = iobj_0->Type->size[1];
15278 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
15279 loop_ub = iobj_0->Type->size[0] * iobj_0->Type->size[1] - 1;
15280 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
15281 switch_expression->data[b_kstr] = iobj_0->Type->data[b_kstr];
15282 }
15283
15284 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
15285 b[b_kstr] = tmp_4[b_kstr];
15286 }
15287
15288 b_bool = false;
15289 if (switch_expression->size[1] == 8) {
15290 b_kstr = 1;
15291 do {
15292 exitg1 = 0;
15293 if (b_kstr - 1 < 8) {
15294 loop_ub = b_kstr - 1;
15295 if (switch_expression->data[loop_ub] != b[loop_ub]) {
15296 exitg1 = 1;
15297 } else {
15298 b_kstr++;
15299 }
15300 } else {
15301 b_bool = true;
15302 exitg1 = 1;
15303 }
15304 } while (exitg1 == 0);
15305 }
15306
15307 if (b_bool) {
15308 b_kstr = 0;
15309 } else {
15310 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
15311 b_0[b_kstr] = tmp_5[b_kstr];
15312 }
15313
15314 b_bool = false;
15315 if (switch_expression->size[1] == 9) {
15316 b_kstr = 1;
15317 do {
15318 exitg1 = 0;
15319 if (b_kstr - 1 < 9) {
15320 loop_ub = b_kstr - 1;
15321 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
15322 exitg1 = 1;
15323 } else {
15324 b_kstr++;
15325 }
15326 } else {
15327 b_bool = true;
15328 exitg1 = 1;
15329 }
15330 } while (exitg1 == 0);
15331 }
15332
15333 if (b_bool) {
15334 b_kstr = 1;
15335 } else {
15336 b_kstr = -1;
15337 }
15338 }
15339
15340 cartesian_trajec_emxFree_char_T(&switch_expression);
15341 switch (b_kstr) {
15342 case 0:
15343 tmp[0] = 0;
15344 tmp[1] = 0;
15345 tmp[2] = 1;
15346 tmp[3] = 0;
15347 tmp[4] = 0;
15348 tmp[5] = 0;
15349 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
15350 msubspace_data[b_kstr] = tmp[b_kstr];
15351 }
15352
15353 poslim_data[0] = -3.1415926535897931;
15354 poslim_data[1] = 3.1415926535897931;
15355 iobj_0->VelocityNumber = 1.0;
15356 iobj_0->PositionNumber = 1.0;
15357 iobj_0->JointAxisInternal[0] = 0.0;
15358 iobj_0->JointAxisInternal[1] = 0.0;
15359 iobj_0->JointAxisInternal[2] = 1.0;
15360 break;
15361
15362 case 1:
15363 tmp[0] = 0;
15364 tmp[1] = 0;
15365 tmp[2] = 0;
15366 tmp[3] = 0;
15367 tmp[4] = 0;
15368 tmp[5] = 1;
15369 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
15370 msubspace_data[b_kstr] = tmp[b_kstr];
15371 }
15372
15373 poslim_data[0] = -0.5;
15374 poslim_data[1] = 0.5;
15375 iobj_0->VelocityNumber = 1.0;
15376 iobj_0->PositionNumber = 1.0;
15377 iobj_0->JointAxisInternal[0] = 0.0;
15378 iobj_0->JointAxisInternal[1] = 0.0;
15379 iobj_0->JointAxisInternal[2] = 1.0;
15380 break;
15381
15382 default:
15383 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
15384 msubspace_data[b_kstr] = 0;
15385 }
15386
15387 poslim_data[0] = 0.0;
15388 poslim_data[1] = 0.0;
15389 iobj_0->VelocityNumber = 0.0;
15390 iobj_0->PositionNumber = 0.0;
15391 iobj_0->JointAxisInternal[0] = 0.0;
15392 iobj_0->JointAxisInternal[1] = 0.0;
15393 iobj_0->JointAxisInternal[2] = 0.0;
15394 break;
15395 }
15396
15397 b_kstr = iobj_0->MotionSubspace->size[0] * iobj_0->MotionSubspace->size[1];
15398 iobj_0->MotionSubspace->size[0] = 6;
15399 iobj_0->MotionSubspace->size[1] = 1;
15400 cartes_emxEnsureCapacity_real_T(iobj_0->MotionSubspace, b_kstr);
15401 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
15402 iobj_0->MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
15403 }
15404
15405 b_kstr = iobj_0->PositionLimitsInternal->size[0] *
15406 iobj_0->PositionLimitsInternal->size[1];
15407 iobj_0->PositionLimitsInternal->size[0] = 1;
15408 iobj_0->PositionLimitsInternal->size[1] = 2;
15409 cartes_emxEnsureCapacity_real_T(iobj_0->PositionLimitsInternal, b_kstr);
15410 for (b_kstr = 0; b_kstr < 2; b_kstr++) {
15411 iobj_0->PositionLimitsInternal->data[b_kstr] = poslim_data[b_kstr];
15412 }
15413
15414 b_kstr = iobj_0->HomePositionInternal->size[0];
15415 iobj_0->HomePositionInternal->size[0] = 1;
15416 cartes_emxEnsureCapacity_real_T(iobj_0->HomePositionInternal, b_kstr);
15417 for (b_kstr = 0; b_kstr < 1; b_kstr++) {
15418 iobj_0->HomePositionInternal->data[0] = 0.0;
15419 }
15420
15421 obj->JointInternal = iobj_0;
15422 obj->Index = -1.0;
15423 obj->ParentIndex = -1.0;
15424 return b_obj;
15425}
15426
15427static w_robotics_manip_internal_Rig_T *c_RigidBody_Rigid_i
15428 (w_robotics_manip_internal_Rig_T *obj, c_rigidBodyJoint_cartesian__a_T *iobj_0)
15429{
15430 w_robotics_manip_internal_Rig_T *b_obj;
15431 int8_T msubspace_data[36];
15432 real_T poslim_data[12];
15433 emxArray_char_T_cartesian_tra_T *switch_expression;
15434 boolean_T b_bool;
15435 int32_T b_kstr;
15436 char_T b[8];
15437 char_T b_0[9];
15438 int32_T loop_ub;
15439 int8_T tmp[6];
15440 static const char_T tmp_0[10] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
15441 '\x02' };
15442
15443 static const int8_T tmp_1[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,
15444 1 };
15445
15446 static const char_T tmp_2[14] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
15447 '\x02', '_', 'j', 'n', 't' };
15448
15449 static const char_T tmp_3[5] = { 'f', 'i', 'x', 'e', 'd' };
15450
15451 static const char_T tmp_4[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
15452
15453 static const char_T tmp_5[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
15454
15455 int32_T exitg1;
15456 b_obj = obj;
15457 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
15458 obj->NameInternal->size[0] = 1;
15459 obj->NameInternal->size[1] = 10;
15460 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);
15461 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
15462 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
15463 }
15464
15465 iobj_0->InTree = false;
15466 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
15467 iobj_0->JointToParentTransform[b_kstr] = tmp_1[b_kstr];
15468 }
15469
15470 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
15471 iobj_0->ChildToJointTransform[b_kstr] = tmp_1[b_kstr];
15472 }
15473
15474 b_kstr = iobj_0->NameInternal->size[0] * iobj_0->NameInternal->size[1];
15475 iobj_0->NameInternal->size[0] = 1;
15476 iobj_0->NameInternal->size[1] = 14;
15477 cartes_emxEnsureCapacity_char_T(iobj_0->NameInternal, b_kstr);
15478 for (b_kstr = 0; b_kstr < 14; b_kstr++) {
15479 iobj_0->NameInternal->data[b_kstr] = tmp_2[b_kstr];
15480 }
15481
15482 b_kstr = iobj_0->Type->size[0] * iobj_0->Type->size[1];
15483 iobj_0->Type->size[0] = 1;
15484 iobj_0->Type->size[1] = 5;
15485 cartes_emxEnsureCapacity_char_T(iobj_0->Type, b_kstr);
15486 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
15487 iobj_0->Type->data[b_kstr] = tmp_3[b_kstr];
15488 }
15489
15490 cartesian_trajec_emxInit_char_T(&switch_expression, 2);
15491 b_kstr = switch_expression->size[0] * switch_expression->size[1];
15492 switch_expression->size[0] = 1;
15493 switch_expression->size[1] = iobj_0->Type->size[1];
15494 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
15495 loop_ub = iobj_0->Type->size[0] * iobj_0->Type->size[1] - 1;
15496 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
15497 switch_expression->data[b_kstr] = iobj_0->Type->data[b_kstr];
15498 }
15499
15500 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
15501 b[b_kstr] = tmp_4[b_kstr];
15502 }
15503
15504 b_bool = false;
15505 if (switch_expression->size[1] == 8) {
15506 b_kstr = 1;
15507 do {
15508 exitg1 = 0;
15509 if (b_kstr - 1 < 8) {
15510 loop_ub = b_kstr - 1;
15511 if (switch_expression->data[loop_ub] != b[loop_ub]) {
15512 exitg1 = 1;
15513 } else {
15514 b_kstr++;
15515 }
15516 } else {
15517 b_bool = true;
15518 exitg1 = 1;
15519 }
15520 } while (exitg1 == 0);
15521 }
15522
15523 if (b_bool) {
15524 b_kstr = 0;
15525 } else {
15526 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
15527 b_0[b_kstr] = tmp_5[b_kstr];
15528 }
15529
15530 b_bool = false;
15531 if (switch_expression->size[1] == 9) {
15532 b_kstr = 1;
15533 do {
15534 exitg1 = 0;
15535 if (b_kstr - 1 < 9) {
15536 loop_ub = b_kstr - 1;
15537 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
15538 exitg1 = 1;
15539 } else {
15540 b_kstr++;
15541 }
15542 } else {
15543 b_bool = true;
15544 exitg1 = 1;
15545 }
15546 } while (exitg1 == 0);
15547 }
15548
15549 if (b_bool) {
15550 b_kstr = 1;
15551 } else {
15552 b_kstr = -1;
15553 }
15554 }
15555
15556 cartesian_trajec_emxFree_char_T(&switch_expression);
15557 switch (b_kstr) {
15558 case 0:
15559 tmp[0] = 0;
15560 tmp[1] = 0;
15561 tmp[2] = 1;
15562 tmp[3] = 0;
15563 tmp[4] = 0;
15564 tmp[5] = 0;
15565 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
15566 msubspace_data[b_kstr] = tmp[b_kstr];
15567 }
15568
15569 poslim_data[0] = -3.1415926535897931;
15570 poslim_data[1] = 3.1415926535897931;
15571 iobj_0->VelocityNumber = 1.0;
15572 iobj_0->PositionNumber = 1.0;
15573 iobj_0->JointAxisInternal[0] = 0.0;
15574 iobj_0->JointAxisInternal[1] = 0.0;
15575 iobj_0->JointAxisInternal[2] = 1.0;
15576 break;
15577
15578 case 1:
15579 tmp[0] = 0;
15580 tmp[1] = 0;
15581 tmp[2] = 0;
15582 tmp[3] = 0;
15583 tmp[4] = 0;
15584 tmp[5] = 1;
15585 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
15586 msubspace_data[b_kstr] = tmp[b_kstr];
15587 }
15588
15589 poslim_data[0] = -0.5;
15590 poslim_data[1] = 0.5;
15591 iobj_0->VelocityNumber = 1.0;
15592 iobj_0->PositionNumber = 1.0;
15593 iobj_0->JointAxisInternal[0] = 0.0;
15594 iobj_0->JointAxisInternal[1] = 0.0;
15595 iobj_0->JointAxisInternal[2] = 1.0;
15596 break;
15597
15598 default:
15599 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
15600 msubspace_data[b_kstr] = 0;
15601 }
15602
15603 poslim_data[0] = 0.0;
15604 poslim_data[1] = 0.0;
15605 iobj_0->VelocityNumber = 0.0;
15606 iobj_0->PositionNumber = 0.0;
15607 iobj_0->JointAxisInternal[0] = 0.0;
15608 iobj_0->JointAxisInternal[1] = 0.0;
15609 iobj_0->JointAxisInternal[2] = 0.0;
15610 break;
15611 }
15612
15613 b_kstr = iobj_0->MotionSubspace->size[0] * iobj_0->MotionSubspace->size[1];
15614 iobj_0->MotionSubspace->size[0] = 6;
15615 iobj_0->MotionSubspace->size[1] = 1;
15616 cartes_emxEnsureCapacity_real_T(iobj_0->MotionSubspace, b_kstr);
15617 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
15618 iobj_0->MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
15619 }
15620
15621 b_kstr = iobj_0->PositionLimitsInternal->size[0] *
15622 iobj_0->PositionLimitsInternal->size[1];
15623 iobj_0->PositionLimitsInternal->size[0] = 1;
15624 iobj_0->PositionLimitsInternal->size[1] = 2;
15625 cartes_emxEnsureCapacity_real_T(iobj_0->PositionLimitsInternal, b_kstr);
15626 for (b_kstr = 0; b_kstr < 2; b_kstr++) {
15627 iobj_0->PositionLimitsInternal->data[b_kstr] = poslim_data[b_kstr];
15628 }
15629
15630 b_kstr = iobj_0->HomePositionInternal->size[0];
15631 iobj_0->HomePositionInternal->size[0] = 1;
15632 cartes_emxEnsureCapacity_real_T(iobj_0->HomePositionInternal, b_kstr);
15633 for (b_kstr = 0; b_kstr < 1; b_kstr++) {
15634 iobj_0->HomePositionInternal->data[0] = 0.0;
15635 }
15636
15637 obj->JointInternal = iobj_0;
15638 obj->Index = -1.0;
15639 obj->ParentIndex = -1.0;
15640 return b_obj;
15641}
15642
15643static w_robotics_manip_internal_Rig_T *c_RigidBody_Rigid_l
15644 (w_robotics_manip_internal_Rig_T *obj, c_rigidBodyJoint_cartesian__a_T *iobj_0)
15645{
15646 w_robotics_manip_internal_Rig_T *b_obj;
15647 int8_T msubspace_data[36];
15648 real_T poslim_data[12];
15649 emxArray_char_T_cartesian_tra_T *switch_expression;
15650 boolean_T b_bool;
15651 int32_T b_kstr;
15652 char_T b[8];
15653 char_T b_0[9];
15654 int32_T loop_ub;
15655 int8_T tmp[6];
15656 static const char_T tmp_0[10] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
15657 '\x03' };
15658
15659 static const int8_T tmp_1[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,
15660 1 };
15661
15662 static const char_T tmp_2[14] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
15663 '\x03', '_', 'j', 'n', 't' };
15664
15665 static const char_T tmp_3[5] = { 'f', 'i', 'x', 'e', 'd' };
15666
15667 static const char_T tmp_4[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
15668
15669 static const char_T tmp_5[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
15670
15671 int32_T exitg1;
15672 b_obj = obj;
15673 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
15674 obj->NameInternal->size[0] = 1;
15675 obj->NameInternal->size[1] = 10;
15676 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);
15677 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
15678 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
15679 }
15680
15681 iobj_0->InTree = false;
15682 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
15683 iobj_0->JointToParentTransform[b_kstr] = tmp_1[b_kstr];
15684 }
15685
15686 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
15687 iobj_0->ChildToJointTransform[b_kstr] = tmp_1[b_kstr];
15688 }
15689
15690 b_kstr = iobj_0->NameInternal->size[0] * iobj_0->NameInternal->size[1];
15691 iobj_0->NameInternal->size[0] = 1;
15692 iobj_0->NameInternal->size[1] = 14;
15693 cartes_emxEnsureCapacity_char_T(iobj_0->NameInternal, b_kstr);
15694 for (b_kstr = 0; b_kstr < 14; b_kstr++) {
15695 iobj_0->NameInternal->data[b_kstr] = tmp_2[b_kstr];
15696 }
15697
15698 b_kstr = iobj_0->Type->size[0] * iobj_0->Type->size[1];
15699 iobj_0->Type->size[0] = 1;
15700 iobj_0->Type->size[1] = 5;
15701 cartes_emxEnsureCapacity_char_T(iobj_0->Type, b_kstr);
15702 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
15703 iobj_0->Type->data[b_kstr] = tmp_3[b_kstr];
15704 }
15705
15706 cartesian_trajec_emxInit_char_T(&switch_expression, 2);
15707 b_kstr = switch_expression->size[0] * switch_expression->size[1];
15708 switch_expression->size[0] = 1;
15709 switch_expression->size[1] = iobj_0->Type->size[1];
15710 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
15711 loop_ub = iobj_0->Type->size[0] * iobj_0->Type->size[1] - 1;
15712 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
15713 switch_expression->data[b_kstr] = iobj_0->Type->data[b_kstr];
15714 }
15715
15716 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
15717 b[b_kstr] = tmp_4[b_kstr];
15718 }
15719
15720 b_bool = false;
15721 if (switch_expression->size[1] == 8) {
15722 b_kstr = 1;
15723 do {
15724 exitg1 = 0;
15725 if (b_kstr - 1 < 8) {
15726 loop_ub = b_kstr - 1;
15727 if (switch_expression->data[loop_ub] != b[loop_ub]) {
15728 exitg1 = 1;
15729 } else {
15730 b_kstr++;
15731 }
15732 } else {
15733 b_bool = true;
15734 exitg1 = 1;
15735 }
15736 } while (exitg1 == 0);
15737 }
15738
15739 if (b_bool) {
15740 b_kstr = 0;
15741 } else {
15742 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
15743 b_0[b_kstr] = tmp_5[b_kstr];
15744 }
15745
15746 b_bool = false;
15747 if (switch_expression->size[1] == 9) {
15748 b_kstr = 1;
15749 do {
15750 exitg1 = 0;
15751 if (b_kstr - 1 < 9) {
15752 loop_ub = b_kstr - 1;
15753 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
15754 exitg1 = 1;
15755 } else {
15756 b_kstr++;
15757 }
15758 } else {
15759 b_bool = true;
15760 exitg1 = 1;
15761 }
15762 } while (exitg1 == 0);
15763 }
15764
15765 if (b_bool) {
15766 b_kstr = 1;
15767 } else {
15768 b_kstr = -1;
15769 }
15770 }
15771
15772 cartesian_trajec_emxFree_char_T(&switch_expression);
15773 switch (b_kstr) {
15774 case 0:
15775 tmp[0] = 0;
15776 tmp[1] = 0;
15777 tmp[2] = 1;
15778 tmp[3] = 0;
15779 tmp[4] = 0;
15780 tmp[5] = 0;
15781 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
15782 msubspace_data[b_kstr] = tmp[b_kstr];
15783 }
15784
15785 poslim_data[0] = -3.1415926535897931;
15786 poslim_data[1] = 3.1415926535897931;
15787 iobj_0->VelocityNumber = 1.0;
15788 iobj_0->PositionNumber = 1.0;
15789 iobj_0->JointAxisInternal[0] = 0.0;
15790 iobj_0->JointAxisInternal[1] = 0.0;
15791 iobj_0->JointAxisInternal[2] = 1.0;
15792 break;
15793
15794 case 1:
15795 tmp[0] = 0;
15796 tmp[1] = 0;
15797 tmp[2] = 0;
15798 tmp[3] = 0;
15799 tmp[4] = 0;
15800 tmp[5] = 1;
15801 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
15802 msubspace_data[b_kstr] = tmp[b_kstr];
15803 }
15804
15805 poslim_data[0] = -0.5;
15806 poslim_data[1] = 0.5;
15807 iobj_0->VelocityNumber = 1.0;
15808 iobj_0->PositionNumber = 1.0;
15809 iobj_0->JointAxisInternal[0] = 0.0;
15810 iobj_0->JointAxisInternal[1] = 0.0;
15811 iobj_0->JointAxisInternal[2] = 1.0;
15812 break;
15813
15814 default:
15815 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
15816 msubspace_data[b_kstr] = 0;
15817 }
15818
15819 poslim_data[0] = 0.0;
15820 poslim_data[1] = 0.0;
15821 iobj_0->VelocityNumber = 0.0;
15822 iobj_0->PositionNumber = 0.0;
15823 iobj_0->JointAxisInternal[0] = 0.0;
15824 iobj_0->JointAxisInternal[1] = 0.0;
15825 iobj_0->JointAxisInternal[2] = 0.0;
15826 break;
15827 }
15828
15829 b_kstr = iobj_0->MotionSubspace->size[0] * iobj_0->MotionSubspace->size[1];
15830 iobj_0->MotionSubspace->size[0] = 6;
15831 iobj_0->MotionSubspace->size[1] = 1;
15832 cartes_emxEnsureCapacity_real_T(iobj_0->MotionSubspace, b_kstr);
15833 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
15834 iobj_0->MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
15835 }
15836
15837 b_kstr = iobj_0->PositionLimitsInternal->size[0] *
15838 iobj_0->PositionLimitsInternal->size[1];
15839 iobj_0->PositionLimitsInternal->size[0] = 1;
15840 iobj_0->PositionLimitsInternal->size[1] = 2;
15841 cartes_emxEnsureCapacity_real_T(iobj_0->PositionLimitsInternal, b_kstr);
15842 for (b_kstr = 0; b_kstr < 2; b_kstr++) {
15843 iobj_0->PositionLimitsInternal->data[b_kstr] = poslim_data[b_kstr];
15844 }
15845
15846 b_kstr = iobj_0->HomePositionInternal->size[0];
15847 iobj_0->HomePositionInternal->size[0] = 1;
15848 cartes_emxEnsureCapacity_real_T(iobj_0->HomePositionInternal, b_kstr);
15849 for (b_kstr = 0; b_kstr < 1; b_kstr++) {
15850 iobj_0->HomePositionInternal->data[0] = 0.0;
15851 }
15852
15853 obj->JointInternal = iobj_0;
15854 obj->Index = -1.0;
15855 obj->ParentIndex = -1.0;
15856 return b_obj;
15857}
15858
15859static w_robotics_manip_internal_Rig_T *c_RigidBody_Rigid_lh
15860 (w_robotics_manip_internal_Rig_T *obj, c_rigidBodyJoint_cartesian__a_T *iobj_0)
15861{
15862 w_robotics_manip_internal_Rig_T *b_obj;
15863 int8_T msubspace_data[36];
15864 real_T poslim_data[12];
15865 emxArray_char_T_cartesian_tra_T *switch_expression;
15866 boolean_T b_bool;
15867 int32_T b_kstr;
15868 char_T b[8];
15869 char_T b_0[9];
15870 int32_T loop_ub;
15871 int8_T tmp[6];
15872 static const char_T tmp_0[10] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
15873 '\x04' };
15874
15875 static const int8_T tmp_1[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,
15876 1 };
15877
15878 static const char_T tmp_2[14] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
15879 '\x04', '_', 'j', 'n', 't' };
15880
15881 static const char_T tmp_3[5] = { 'f', 'i', 'x', 'e', 'd' };
15882
15883 static const char_T tmp_4[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
15884
15885 static const char_T tmp_5[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
15886
15887 int32_T exitg1;
15888 b_obj = obj;
15889 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
15890 obj->NameInternal->size[0] = 1;
15891 obj->NameInternal->size[1] = 10;
15892 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);
15893 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
15894 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
15895 }
15896
15897 iobj_0->InTree = false;
15898 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
15899 iobj_0->JointToParentTransform[b_kstr] = tmp_1[b_kstr];
15900 }
15901
15902 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
15903 iobj_0->ChildToJointTransform[b_kstr] = tmp_1[b_kstr];
15904 }
15905
15906 b_kstr = iobj_0->NameInternal->size[0] * iobj_0->NameInternal->size[1];
15907 iobj_0->NameInternal->size[0] = 1;
15908 iobj_0->NameInternal->size[1] = 14;
15909 cartes_emxEnsureCapacity_char_T(iobj_0->NameInternal, b_kstr);
15910 for (b_kstr = 0; b_kstr < 14; b_kstr++) {
15911 iobj_0->NameInternal->data[b_kstr] = tmp_2[b_kstr];
15912 }
15913
15914 b_kstr = iobj_0->Type->size[0] * iobj_0->Type->size[1];
15915 iobj_0->Type->size[0] = 1;
15916 iobj_0->Type->size[1] = 5;
15917 cartes_emxEnsureCapacity_char_T(iobj_0->Type, b_kstr);
15918 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
15919 iobj_0->Type->data[b_kstr] = tmp_3[b_kstr];
15920 }
15921
15922 cartesian_trajec_emxInit_char_T(&switch_expression, 2);
15923 b_kstr = switch_expression->size[0] * switch_expression->size[1];
15924 switch_expression->size[0] = 1;
15925 switch_expression->size[1] = iobj_0->Type->size[1];
15926 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
15927 loop_ub = iobj_0->Type->size[0] * iobj_0->Type->size[1] - 1;
15928 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
15929 switch_expression->data[b_kstr] = iobj_0->Type->data[b_kstr];
15930 }
15931
15932 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
15933 b[b_kstr] = tmp_4[b_kstr];
15934 }
15935
15936 b_bool = false;
15937 if (switch_expression->size[1] == 8) {
15938 b_kstr = 1;
15939 do {
15940 exitg1 = 0;
15941 if (b_kstr - 1 < 8) {
15942 loop_ub = b_kstr - 1;
15943 if (switch_expression->data[loop_ub] != b[loop_ub]) {
15944 exitg1 = 1;
15945 } else {
15946 b_kstr++;
15947 }
15948 } else {
15949 b_bool = true;
15950 exitg1 = 1;
15951 }
15952 } while (exitg1 == 0);
15953 }
15954
15955 if (b_bool) {
15956 b_kstr = 0;
15957 } else {
15958 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
15959 b_0[b_kstr] = tmp_5[b_kstr];
15960 }
15961
15962 b_bool = false;
15963 if (switch_expression->size[1] == 9) {
15964 b_kstr = 1;
15965 do {
15966 exitg1 = 0;
15967 if (b_kstr - 1 < 9) {
15968 loop_ub = b_kstr - 1;
15969 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
15970 exitg1 = 1;
15971 } else {
15972 b_kstr++;
15973 }
15974 } else {
15975 b_bool = true;
15976 exitg1 = 1;
15977 }
15978 } while (exitg1 == 0);
15979 }
15980
15981 if (b_bool) {
15982 b_kstr = 1;
15983 } else {
15984 b_kstr = -1;
15985 }
15986 }
15987
15988 cartesian_trajec_emxFree_char_T(&switch_expression);
15989 switch (b_kstr) {
15990 case 0:
15991 tmp[0] = 0;
15992 tmp[1] = 0;
15993 tmp[2] = 1;
15994 tmp[3] = 0;
15995 tmp[4] = 0;
15996 tmp[5] = 0;
15997 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
15998 msubspace_data[b_kstr] = tmp[b_kstr];
15999 }
16000
16001 poslim_data[0] = -3.1415926535897931;
16002 poslim_data[1] = 3.1415926535897931;
16003 iobj_0->VelocityNumber = 1.0;
16004 iobj_0->PositionNumber = 1.0;
16005 iobj_0->JointAxisInternal[0] = 0.0;
16006 iobj_0->JointAxisInternal[1] = 0.0;
16007 iobj_0->JointAxisInternal[2] = 1.0;
16008 break;
16009
16010 case 1:
16011 tmp[0] = 0;
16012 tmp[1] = 0;
16013 tmp[2] = 0;
16014 tmp[3] = 0;
16015 tmp[4] = 0;
16016 tmp[5] = 1;
16017 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
16018 msubspace_data[b_kstr] = tmp[b_kstr];
16019 }
16020
16021 poslim_data[0] = -0.5;
16022 poslim_data[1] = 0.5;
16023 iobj_0->VelocityNumber = 1.0;
16024 iobj_0->PositionNumber = 1.0;
16025 iobj_0->JointAxisInternal[0] = 0.0;
16026 iobj_0->JointAxisInternal[1] = 0.0;
16027 iobj_0->JointAxisInternal[2] = 1.0;
16028 break;
16029
16030 default:
16031 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
16032 msubspace_data[b_kstr] = 0;
16033 }
16034
16035 poslim_data[0] = 0.0;
16036 poslim_data[1] = 0.0;
16037 iobj_0->VelocityNumber = 0.0;
16038 iobj_0->PositionNumber = 0.0;
16039 iobj_0->JointAxisInternal[0] = 0.0;
16040 iobj_0->JointAxisInternal[1] = 0.0;
16041 iobj_0->JointAxisInternal[2] = 0.0;
16042 break;
16043 }
16044
16045 b_kstr = iobj_0->MotionSubspace->size[0] * iobj_0->MotionSubspace->size[1];
16046 iobj_0->MotionSubspace->size[0] = 6;
16047 iobj_0->MotionSubspace->size[1] = 1;
16048 cartes_emxEnsureCapacity_real_T(iobj_0->MotionSubspace, b_kstr);
16049 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
16050 iobj_0->MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
16051 }
16052
16053 b_kstr = iobj_0->PositionLimitsInternal->size[0] *
16054 iobj_0->PositionLimitsInternal->size[1];
16055 iobj_0->PositionLimitsInternal->size[0] = 1;
16056 iobj_0->PositionLimitsInternal->size[1] = 2;
16057 cartes_emxEnsureCapacity_real_T(iobj_0->PositionLimitsInternal, b_kstr);
16058 for (b_kstr = 0; b_kstr < 2; b_kstr++) {
16059 iobj_0->PositionLimitsInternal->data[b_kstr] = poslim_data[b_kstr];
16060 }
16061
16062 b_kstr = iobj_0->HomePositionInternal->size[0];
16063 iobj_0->HomePositionInternal->size[0] = 1;
16064 cartes_emxEnsureCapacity_real_T(iobj_0->HomePositionInternal, b_kstr);
16065 for (b_kstr = 0; b_kstr < 1; b_kstr++) {
16066 iobj_0->HomePositionInternal->data[0] = 0.0;
16067 }
16068
16069 obj->JointInternal = iobj_0;
16070 obj->Index = -1.0;
16071 obj->ParentIndex = -1.0;
16072 return b_obj;
16073}
16074
16075static w_robotics_manip_internal_Rig_T *c_RigidBody_Rigid_k
16076 (w_robotics_manip_internal_Rig_T *obj, c_rigidBodyJoint_cartesian__a_T *iobj_0)
16077{
16078 w_robotics_manip_internal_Rig_T *b_obj;
16079 int8_T msubspace_data[36];
16080 real_T poslim_data[12];
16081 emxArray_char_T_cartesian_tra_T *switch_expression;
16082 boolean_T b_bool;
16083 int32_T b_kstr;
16084 char_T b[8];
16085 char_T b_0[9];
16086 int32_T loop_ub;
16087 int8_T tmp[6];
16088 static const char_T tmp_0[10] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
16089 '\x05' };
16090
16091 static const int8_T tmp_1[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,
16092 1 };
16093
16094 static const char_T tmp_2[14] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
16095 '\x05', '_', 'j', 'n', 't' };
16096
16097 static const char_T tmp_3[5] = { 'f', 'i', 'x', 'e', 'd' };
16098
16099 static const char_T tmp_4[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
16100
16101 static const char_T tmp_5[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
16102
16103 int32_T exitg1;
16104 b_obj = obj;
16105 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
16106 obj->NameInternal->size[0] = 1;
16107 obj->NameInternal->size[1] = 10;
16108 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);
16109 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
16110 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
16111 }
16112
16113 iobj_0->InTree = false;
16114 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
16115 iobj_0->JointToParentTransform[b_kstr] = tmp_1[b_kstr];
16116 }
16117
16118 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
16119 iobj_0->ChildToJointTransform[b_kstr] = tmp_1[b_kstr];
16120 }
16121
16122 b_kstr = iobj_0->NameInternal->size[0] * iobj_0->NameInternal->size[1];
16123 iobj_0->NameInternal->size[0] = 1;
16124 iobj_0->NameInternal->size[1] = 14;
16125 cartes_emxEnsureCapacity_char_T(iobj_0->NameInternal, b_kstr);
16126 for (b_kstr = 0; b_kstr < 14; b_kstr++) {
16127 iobj_0->NameInternal->data[b_kstr] = tmp_2[b_kstr];
16128 }
16129
16130 b_kstr = iobj_0->Type->size[0] * iobj_0->Type->size[1];
16131 iobj_0->Type->size[0] = 1;
16132 iobj_0->Type->size[1] = 5;
16133 cartes_emxEnsureCapacity_char_T(iobj_0->Type, b_kstr);
16134 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
16135 iobj_0->Type->data[b_kstr] = tmp_3[b_kstr];
16136 }
16137
16138 cartesian_trajec_emxInit_char_T(&switch_expression, 2);
16139 b_kstr = switch_expression->size[0] * switch_expression->size[1];
16140 switch_expression->size[0] = 1;
16141 switch_expression->size[1] = iobj_0->Type->size[1];
16142 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
16143 loop_ub = iobj_0->Type->size[0] * iobj_0->Type->size[1] - 1;
16144 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
16145 switch_expression->data[b_kstr] = iobj_0->Type->data[b_kstr];
16146 }
16147
16148 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
16149 b[b_kstr] = tmp_4[b_kstr];
16150 }
16151
16152 b_bool = false;
16153 if (switch_expression->size[1] == 8) {
16154 b_kstr = 1;
16155 do {
16156 exitg1 = 0;
16157 if (b_kstr - 1 < 8) {
16158 loop_ub = b_kstr - 1;
16159 if (switch_expression->data[loop_ub] != b[loop_ub]) {
16160 exitg1 = 1;
16161 } else {
16162 b_kstr++;
16163 }
16164 } else {
16165 b_bool = true;
16166 exitg1 = 1;
16167 }
16168 } while (exitg1 == 0);
16169 }
16170
16171 if (b_bool) {
16172 b_kstr = 0;
16173 } else {
16174 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
16175 b_0[b_kstr] = tmp_5[b_kstr];
16176 }
16177
16178 b_bool = false;
16179 if (switch_expression->size[1] == 9) {
16180 b_kstr = 1;
16181 do {
16182 exitg1 = 0;
16183 if (b_kstr - 1 < 9) {
16184 loop_ub = b_kstr - 1;
16185 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
16186 exitg1 = 1;
16187 } else {
16188 b_kstr++;
16189 }
16190 } else {
16191 b_bool = true;
16192 exitg1 = 1;
16193 }
16194 } while (exitg1 == 0);
16195 }
16196
16197 if (b_bool) {
16198 b_kstr = 1;
16199 } else {
16200 b_kstr = -1;
16201 }
16202 }
16203
16204 cartesian_trajec_emxFree_char_T(&switch_expression);
16205 switch (b_kstr) {
16206 case 0:
16207 tmp[0] = 0;
16208 tmp[1] = 0;
16209 tmp[2] = 1;
16210 tmp[3] = 0;
16211 tmp[4] = 0;
16212 tmp[5] = 0;
16213 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
16214 msubspace_data[b_kstr] = tmp[b_kstr];
16215 }
16216
16217 poslim_data[0] = -3.1415926535897931;
16218 poslim_data[1] = 3.1415926535897931;
16219 iobj_0->VelocityNumber = 1.0;
16220 iobj_0->PositionNumber = 1.0;
16221 iobj_0->JointAxisInternal[0] = 0.0;
16222 iobj_0->JointAxisInternal[1] = 0.0;
16223 iobj_0->JointAxisInternal[2] = 1.0;
16224 break;
16225
16226 case 1:
16227 tmp[0] = 0;
16228 tmp[1] = 0;
16229 tmp[2] = 0;
16230 tmp[3] = 0;
16231 tmp[4] = 0;
16232 tmp[5] = 1;
16233 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
16234 msubspace_data[b_kstr] = tmp[b_kstr];
16235 }
16236
16237 poslim_data[0] = -0.5;
16238 poslim_data[1] = 0.5;
16239 iobj_0->VelocityNumber = 1.0;
16240 iobj_0->PositionNumber = 1.0;
16241 iobj_0->JointAxisInternal[0] = 0.0;
16242 iobj_0->JointAxisInternal[1] = 0.0;
16243 iobj_0->JointAxisInternal[2] = 1.0;
16244 break;
16245
16246 default:
16247 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
16248 msubspace_data[b_kstr] = 0;
16249 }
16250
16251 poslim_data[0] = 0.0;
16252 poslim_data[1] = 0.0;
16253 iobj_0->VelocityNumber = 0.0;
16254 iobj_0->PositionNumber = 0.0;
16255 iobj_0->JointAxisInternal[0] = 0.0;
16256 iobj_0->JointAxisInternal[1] = 0.0;
16257 iobj_0->JointAxisInternal[2] = 0.0;
16258 break;
16259 }
16260
16261 b_kstr = iobj_0->MotionSubspace->size[0] * iobj_0->MotionSubspace->size[1];
16262 iobj_0->MotionSubspace->size[0] = 6;
16263 iobj_0->MotionSubspace->size[1] = 1;
16264 cartes_emxEnsureCapacity_real_T(iobj_0->MotionSubspace, b_kstr);
16265 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
16266 iobj_0->MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
16267 }
16268
16269 b_kstr = iobj_0->PositionLimitsInternal->size[0] *
16270 iobj_0->PositionLimitsInternal->size[1];
16271 iobj_0->PositionLimitsInternal->size[0] = 1;
16272 iobj_0->PositionLimitsInternal->size[1] = 2;
16273 cartes_emxEnsureCapacity_real_T(iobj_0->PositionLimitsInternal, b_kstr);
16274 for (b_kstr = 0; b_kstr < 2; b_kstr++) {
16275 iobj_0->PositionLimitsInternal->data[b_kstr] = poslim_data[b_kstr];
16276 }
16277
16278 b_kstr = iobj_0->HomePositionInternal->size[0];
16279 iobj_0->HomePositionInternal->size[0] = 1;
16280 cartes_emxEnsureCapacity_real_T(iobj_0->HomePositionInternal, b_kstr);
16281 for (b_kstr = 0; b_kstr < 1; b_kstr++) {
16282 iobj_0->HomePositionInternal->data[0] = 0.0;
16283 }
16284
16285 obj->JointInternal = iobj_0;
16286 obj->Index = -1.0;
16287 obj->ParentIndex = -1.0;
16288 return b_obj;
16289}
16290
16291static void ca_RigidBodyTree_clearAllBodies(x_robotics_manip_internal_Rig_T *obj,
16292 w_robotics_manip_internal_Rig_T *iobj_0, w_robotics_manip_internal_Rig_T
16293 *iobj_1, w_robotics_manip_internal_Rig_T *iobj_2,
16294 w_robotics_manip_internal_Rig_T *iobj_3, w_robotics_manip_internal_Rig_T
16295 *iobj_4, w_robotics_manip_internal_Rig_T *iobj_5,
16296 w_robotics_manip_internal_Rig_T *iobj_6, c_rigidBodyJoint_cartesian__a_T
16297 *iobj_7, c_rigidBodyJoint_cartesian__a_T *iobj_8,
16298 c_rigidBodyJoint_cartesian__a_T *iobj_9, c_rigidBodyJoint_cartesian__a_T
16299 *iobj_10, c_rigidBodyJoint_cartesian__a_T *iobj_11,
16300 c_rigidBodyJoint_cartesian__a_T *iobj_12, c_rigidBodyJoint_cartesian__a_T
16301 *iobj_13, c_rigidBodyJoint_cartesian__a_T *iobj_14,
16302 w_robotics_manip_internal_Rig_T *iobj_15)
16303{
16304 emxArray_char_T_cartesian_tra_T *switch_expression;
16305 static const char_T tmp[10] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
16306 '\x06' };
16307
16308 static const int8_T tmp_0[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,
16309 1 };
16310
16311 static const char_T tmp_1[14] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
16312 '\x06', '_', 'j', 'n', 't' };
16313
16314 static const char_T tmp_2[5] = { 'f', 'i', 'x', 'e', 'd' };
16315
16316 static const char_T tmp_3[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
16317
16318 static const char_T tmp_4[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
16319
16320 static const char_T tmp_5[10] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
16321 '\x07' };
16322
16323 static const char_T tmp_6[14] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
16324 '\x07', '_', 'j', 'n', 't' };
16325
16326 static const char_T tmp_7[10] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
16327 '\x08' };
16328
16329 static const char_T tmp_8[14] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',
16330 '\x08', '_', 'j', 'n', 't' };
16331
16332 int32_T exitg1;
16333 obj->Bodies[0] = c_RigidBody_Rigid_b(iobj_15, iobj_14);
16334 obj->Bodies[1] = c_RigidBody_Rigid_i(iobj_0, iobj_7);
16335 obj->Bodies[2] = c_RigidBody_Rigid_l(iobj_1, iobj_8);
16336 obj->Bodies[3] = c_RigidBody_Rigid_lh(iobj_2, iobj_9);
16337 obj->Bodies[4] = c_RigidBody_Rigid_k(iobj_3, iobj_10);
16338 cartesian_trajectory_planner_B.b_kstr_m = iobj_4->NameInternal->size[0] *
16339 iobj_4->NameInternal->size[1];
16340 iobj_4->NameInternal->size[0] = 1;
16341 iobj_4->NameInternal->size[1] = 10;
16342 cartes_emxEnsureCapacity_char_T(iobj_4->NameInternal,
16343 cartesian_trajectory_planner_B.b_kstr_m);
16344 for (cartesian_trajectory_planner_B.b_kstr_m = 0;
16345 cartesian_trajectory_planner_B.b_kstr_m < 10;
16346 cartesian_trajectory_planner_B.b_kstr_m++) {
16347 iobj_4->NameInternal->data[cartesian_trajectory_planner_B.b_kstr_m] =
16348 tmp[cartesian_trajectory_planner_B.b_kstr_m];
16349 }
16350
16351 iobj_11->InTree = false;
16352 for (cartesian_trajectory_planner_B.b_kstr_m = 0;
16353 cartesian_trajectory_planner_B.b_kstr_m < 16;
16354 cartesian_trajectory_planner_B.b_kstr_m++) {
16355 iobj_11->JointToParentTransform[cartesian_trajectory_planner_B.b_kstr_m] =
16356 tmp_0[cartesian_trajectory_planner_B.b_kstr_m];
16357 }
16358
16359 for (cartesian_trajectory_planner_B.b_kstr_m = 0;
16360 cartesian_trajectory_planner_B.b_kstr_m < 16;
16361 cartesian_trajectory_planner_B.b_kstr_m++) {
16362 iobj_11->ChildToJointTransform[cartesian_trajectory_planner_B.b_kstr_m] =
16363 tmp_0[cartesian_trajectory_planner_B.b_kstr_m];
16364 }
16365
16366 cartesian_trajectory_planner_B.b_kstr_m = iobj_11->NameInternal->size[0] *
16367 iobj_11->NameInternal->size[1];
16368 iobj_11->NameInternal->size[0] = 1;
16369 iobj_11->NameInternal->size[1] = 14;
16370 cartes_emxEnsureCapacity_char_T(iobj_11->NameInternal,
16371 cartesian_trajectory_planner_B.b_kstr_m);
16372 for (cartesian_trajectory_planner_B.b_kstr_m = 0;
16373 cartesian_trajectory_planner_B.b_kstr_m < 14;
16374 cartesian_trajectory_planner_B.b_kstr_m++) {
16375 iobj_11->NameInternal->data[cartesian_trajectory_planner_B.b_kstr_m] =
16376 tmp_1[cartesian_trajectory_planner_B.b_kstr_m];
16377 }
16378
16379 cartesian_trajectory_planner_B.b_kstr_m = iobj_11->Type->size[0] *
16380 iobj_11->Type->size[1];
16381 iobj_11->Type->size[0] = 1;
16382 iobj_11->Type->size[1] = 5;
16383 cartes_emxEnsureCapacity_char_T(iobj_11->Type,
16384 cartesian_trajectory_planner_B.b_kstr_m);
16385 for (cartesian_trajectory_planner_B.b_kstr_m = 0;
16386 cartesian_trajectory_planner_B.b_kstr_m < 5;
16387 cartesian_trajectory_planner_B.b_kstr_m++) {
16388 iobj_11->Type->data[cartesian_trajectory_planner_B.b_kstr_m] =
16389 tmp_2[cartesian_trajectory_planner_B.b_kstr_m];
16390 }
16391
16392 cartesian_trajec_emxInit_char_T(&switch_expression, 2);
16393 cartesian_trajectory_planner_B.b_kstr_m = switch_expression->size[0] *
16394 switch_expression->size[1];
16395 switch_expression->size[0] = 1;
16396 switch_expression->size[1] = iobj_11->Type->size[1];
16397 cartes_emxEnsureCapacity_char_T(switch_expression,
16398 cartesian_trajectory_planner_B.b_kstr_m);
16399 cartesian_trajectory_planner_B.loop_ub_o = iobj_11->Type->size[0] *
16400 iobj_11->Type->size[1] - 1;
16401 for (cartesian_trajectory_planner_B.b_kstr_m = 0;
16402 cartesian_trajectory_planner_B.b_kstr_m <=
16403 cartesian_trajectory_planner_B.loop_ub_o;
16404 cartesian_trajectory_planner_B.b_kstr_m++) {
16405 switch_expression->data[cartesian_trajectory_planner_B.b_kstr_m] =
16406 iobj_11->Type->data[cartesian_trajectory_planner_B.b_kstr_m];
16407 }
16408
16409 for (cartesian_trajectory_planner_B.b_kstr_m = 0;
16410 cartesian_trajectory_planner_B.b_kstr_m < 8;
16411 cartesian_trajectory_planner_B.b_kstr_m++) {
16412 cartesian_trajectory_planner_B.b_nj[cartesian_trajectory_planner_B.b_kstr_m]
16413 = tmp_3[cartesian_trajectory_planner_B.b_kstr_m];
16414 }
16415
16416 cartesian_trajectory_planner_B.b_bool_pn = false;
16417 if (switch_expression->size[1] == 8) {
16418 cartesian_trajectory_planner_B.b_kstr_m = 1;
16419 do {
16420 exitg1 = 0;
16421 if (cartesian_trajectory_planner_B.b_kstr_m - 1 < 8) {
16422 cartesian_trajectory_planner_B.loop_ub_o =
16423 cartesian_trajectory_planner_B.b_kstr_m - 1;
16424 if (switch_expression->data[cartesian_trajectory_planner_B.loop_ub_o] !=
16425 cartesian_trajectory_planner_B.b_nj[cartesian_trajectory_planner_B.loop_ub_o])
16426 {
16427 exitg1 = 1;
16428 } else {
16429 cartesian_trajectory_planner_B.b_kstr_m++;
16430 }
16431 } else {
16432 cartesian_trajectory_planner_B.b_bool_pn = true;
16433 exitg1 = 1;
16434 }
16435 } while (exitg1 == 0);
16436 }
16437
16438 if (cartesian_trajectory_planner_B.b_bool_pn) {
16439 cartesian_trajectory_planner_B.b_kstr_m = 0;
16440 } else {
16441 for (cartesian_trajectory_planner_B.b_kstr_m = 0;
16442 cartesian_trajectory_planner_B.b_kstr_m < 9;
16443 cartesian_trajectory_planner_B.b_kstr_m++) {
16444 cartesian_trajectory_planner_B.b_g[cartesian_trajectory_planner_B.b_kstr_m]
16445 = tmp_4[cartesian_trajectory_planner_B.b_kstr_m];
16446 }
16447
16448 cartesian_trajectory_planner_B.b_bool_pn = false;
16449 if (switch_expression->size[1] == 9) {
16450 cartesian_trajectory_planner_B.b_kstr_m = 1;
16451 do {
16452 exitg1 = 0;
16453 if (cartesian_trajectory_planner_B.b_kstr_m - 1 < 9) {
16454 cartesian_trajectory_planner_B.loop_ub_o =
16455 cartesian_trajectory_planner_B.b_kstr_m - 1;
16456 if (switch_expression->data[cartesian_trajectory_planner_B.loop_ub_o]
16457 !=
16458 cartesian_trajectory_planner_B.b_g[cartesian_trajectory_planner_B.loop_ub_o])
16459 {
16460 exitg1 = 1;
16461 } else {
16462 cartesian_trajectory_planner_B.b_kstr_m++;
16463 }
16464 } else {
16465 cartesian_trajectory_planner_B.b_bool_pn = true;
16466 exitg1 = 1;
16467 }
16468 } while (exitg1 == 0);
16469 }
16470
16471 if (cartesian_trajectory_planner_B.b_bool_pn) {
16472 cartesian_trajectory_planner_B.b_kstr_m = 1;
16473 } else {
16474 cartesian_trajectory_planner_B.b_kstr_m = -1;
16475 }
16476 }
16477
16478 switch (cartesian_trajectory_planner_B.b_kstr_m) {
16479 case 0:
16480 cartesian_trajectory_planner_B.iv1[0] = 0;
16481 cartesian_trajectory_planner_B.iv1[1] = 0;
16482 cartesian_trajectory_planner_B.iv1[2] = 1;
16483 cartesian_trajectory_planner_B.iv1[3] = 0;
16484 cartesian_trajectory_planner_B.iv1[4] = 0;
16485 cartesian_trajectory_planner_B.iv1[5] = 0;
16486 for (cartesian_trajectory_planner_B.b_kstr_m = 0;
16487 cartesian_trajectory_planner_B.b_kstr_m < 6;
16488 cartesian_trajectory_planner_B.b_kstr_m++) {
16489 cartesian_trajectory_planner_B.msubspace_data_i[cartesian_trajectory_planner_B.b_kstr_m]
16490 =
16491 cartesian_trajectory_planner_B.iv1[cartesian_trajectory_planner_B.b_kstr_m];
16492 }
16493
16494 cartesian_trajectory_planner_B.poslim_data_a[0] = -3.1415926535897931;
16495 cartesian_trajectory_planner_B.poslim_data_a[1] = 3.1415926535897931;
16496 iobj_11->VelocityNumber = 1.0;
16497 iobj_11->PositionNumber = 1.0;
16498 iobj_11->JointAxisInternal[0] = 0.0;
16499 iobj_11->JointAxisInternal[1] = 0.0;
16500 iobj_11->JointAxisInternal[2] = 1.0;
16501 break;
16502
16503 case 1:
16504 cartesian_trajectory_planner_B.iv1[0] = 0;
16505 cartesian_trajectory_planner_B.iv1[1] = 0;
16506 cartesian_trajectory_planner_B.iv1[2] = 0;
16507 cartesian_trajectory_planner_B.iv1[3] = 0;
16508 cartesian_trajectory_planner_B.iv1[4] = 0;
16509 cartesian_trajectory_planner_B.iv1[5] = 1;
16510 for (cartesian_trajectory_planner_B.b_kstr_m = 0;
16511 cartesian_trajectory_planner_B.b_kstr_m < 6;
16512 cartesian_trajectory_planner_B.b_kstr_m++) {
16513 cartesian_trajectory_planner_B.msubspace_data_i[cartesian_trajectory_planner_B.b_kstr_m]
16514 =
16515 cartesian_trajectory_planner_B.iv1[cartesian_trajectory_planner_B.b_kstr_m];
16516 }
16517
16518 cartesian_trajectory_planner_B.poslim_data_a[0] = -0.5;
16519 cartesian_trajectory_planner_B.poslim_data_a[1] = 0.5;
16520 iobj_11->VelocityNumber = 1.0;
16521 iobj_11->PositionNumber = 1.0;
16522 iobj_11->JointAxisInternal[0] = 0.0;
16523 iobj_11->JointAxisInternal[1] = 0.0;
16524 iobj_11->JointAxisInternal[2] = 1.0;
16525 break;
16526
16527 default:
16528 for (cartesian_trajectory_planner_B.b_kstr_m = 0;
16529 cartesian_trajectory_planner_B.b_kstr_m < 6;
16530 cartesian_trajectory_planner_B.b_kstr_m++) {
16531 cartesian_trajectory_planner_B.msubspace_data_i[cartesian_trajectory_planner_B.b_kstr_m]
16532 = 0;
16533 }
16534
16535 cartesian_trajectory_planner_B.poslim_data_a[0] = 0.0;
16536 cartesian_trajectory_planner_B.poslim_data_a[1] = 0.0;
16537 iobj_11->VelocityNumber = 0.0;
16538 iobj_11->PositionNumber = 0.0;
16539 iobj_11->JointAxisInternal[0] = 0.0;
16540 iobj_11->JointAxisInternal[1] = 0.0;
16541 iobj_11->JointAxisInternal[2] = 0.0;
16542 break;
16543 }
16544
16545 cartesian_trajectory_planner_B.b_kstr_m = iobj_11->MotionSubspace->size[0] *
16546 iobj_11->MotionSubspace->size[1];
16547 iobj_11->MotionSubspace->size[0] = 6;
16548 iobj_11->MotionSubspace->size[1] = 1;
16549 cartes_emxEnsureCapacity_real_T(iobj_11->MotionSubspace,
16550 cartesian_trajectory_planner_B.b_kstr_m);
16551 for (cartesian_trajectory_planner_B.b_kstr_m = 0;
16552 cartesian_trajectory_planner_B.b_kstr_m < 6;
16553 cartesian_trajectory_planner_B.b_kstr_m++) {
16554 iobj_11->MotionSubspace->data[cartesian_trajectory_planner_B.b_kstr_m] =
16555 cartesian_trajectory_planner_B.msubspace_data_i[cartesian_trajectory_planner_B.b_kstr_m];
16556 }
16557
16558 cartesian_trajectory_planner_B.b_kstr_m = iobj_11->
16559 PositionLimitsInternal->size[0] * iobj_11->PositionLimitsInternal->size[1];
16560 iobj_11->PositionLimitsInternal->size[0] = 1;
16561 iobj_11->PositionLimitsInternal->size[1] = 2;
16562 cartes_emxEnsureCapacity_real_T(iobj_11->PositionLimitsInternal,
16563 cartesian_trajectory_planner_B.b_kstr_m);
16564 for (cartesian_trajectory_planner_B.b_kstr_m = 0;
16565 cartesian_trajectory_planner_B.b_kstr_m < 2;
16566 cartesian_trajectory_planner_B.b_kstr_m++) {
16567 iobj_11->PositionLimitsInternal->
16568 data[cartesian_trajectory_planner_B.b_kstr_m] =
16569 cartesian_trajectory_planner_B.poslim_data_a[cartesian_trajectory_planner_B.b_kstr_m];
16570 }
16571
16572 cartesian_trajectory_planner_B.b_kstr_m = iobj_11->HomePositionInternal->size
16573 [0];
16574 iobj_11->HomePositionInternal->size[0] = 1;
16575 cartes_emxEnsureCapacity_real_T(iobj_11->HomePositionInternal,
16576 cartesian_trajectory_planner_B.b_kstr_m);
16577 for (cartesian_trajectory_planner_B.b_kstr_m = 0;
16578 cartesian_trajectory_planner_B.b_kstr_m < 1;
16579 cartesian_trajectory_planner_B.b_kstr_m++) {
16580 iobj_11->HomePositionInternal->data[0] = 0.0;
16581 }
16582
16583 iobj_4->JointInternal = iobj_11;
16584 iobj_4->Index = -1.0;
16585 iobj_4->ParentIndex = -1.0;
16586 obj->Bodies[5] = iobj_4;
16587 cartesian_trajectory_planner_B.b_kstr_m = iobj_5->NameInternal->size[0] *
16588 iobj_5->NameInternal->size[1];
16589 iobj_5->NameInternal->size[0] = 1;
16590 iobj_5->NameInternal->size[1] = 10;
16591 cartes_emxEnsureCapacity_char_T(iobj_5->NameInternal,
16592 cartesian_trajectory_planner_B.b_kstr_m);
16593 for (cartesian_trajectory_planner_B.b_kstr_m = 0;
16594 cartesian_trajectory_planner_B.b_kstr_m < 10;
16595 cartesian_trajectory_planner_B.b_kstr_m++) {
16596 iobj_5->NameInternal->data[cartesian_trajectory_planner_B.b_kstr_m] =
16597 tmp_5[cartesian_trajectory_planner_B.b_kstr_m];
16598 }
16599
16600 iobj_12->InTree = false;
16601 for (cartesian_trajectory_planner_B.b_kstr_m = 0;
16602 cartesian_trajectory_planner_B.b_kstr_m < 16;
16603 cartesian_trajectory_planner_B.b_kstr_m++) {
16604 iobj_12->JointToParentTransform[cartesian_trajectory_planner_B.b_kstr_m] =
16605 tmp_0[cartesian_trajectory_planner_B.b_kstr_m];
16606 }
16607
16608 for (cartesian_trajectory_planner_B.b_kstr_m = 0;
16609 cartesian_trajectory_planner_B.b_kstr_m < 16;
16610 cartesian_trajectory_planner_B.b_kstr_m++) {
16611 iobj_12->ChildToJointTransform[cartesian_trajectory_planner_B.b_kstr_m] =
16612 tmp_0[cartesian_trajectory_planner_B.b_kstr_m];
16613 }
16614
16615 cartesian_trajectory_planner_B.b_kstr_m = iobj_12->NameInternal->size[0] *
16616 iobj_12->NameInternal->size[1];
16617 iobj_12->NameInternal->size[0] = 1;
16618 iobj_12->NameInternal->size[1] = 14;
16619 cartes_emxEnsureCapacity_char_T(iobj_12->NameInternal,
16620 cartesian_trajectory_planner_B.b_kstr_m);
16621 for (cartesian_trajectory_planner_B.b_kstr_m = 0;
16622 cartesian_trajectory_planner_B.b_kstr_m < 14;
16623 cartesian_trajectory_planner_B.b_kstr_m++) {
16624 iobj_12->NameInternal->data[cartesian_trajectory_planner_B.b_kstr_m] =
16625 tmp_6[cartesian_trajectory_planner_B.b_kstr_m];
16626 }
16627
16628 cartesian_trajectory_planner_B.b_kstr_m = iobj_12->Type->size[0] *
16629 iobj_12->Type->size[1];
16630 iobj_12->Type->size[0] = 1;
16631 iobj_12->Type->size[1] = 5;
16632 cartes_emxEnsureCapacity_char_T(iobj_12->Type,
16633 cartesian_trajectory_planner_B.b_kstr_m);
16634 for (cartesian_trajectory_planner_B.b_kstr_m = 0;
16635 cartesian_trajectory_planner_B.b_kstr_m < 5;
16636 cartesian_trajectory_planner_B.b_kstr_m++) {
16637 iobj_12->Type->data[cartesian_trajectory_planner_B.b_kstr_m] =
16638 tmp_2[cartesian_trajectory_planner_B.b_kstr_m];
16639 }
16640
16641 cartesian_trajectory_planner_B.b_kstr_m = switch_expression->size[0] *
16642 switch_expression->size[1];
16643 switch_expression->size[0] = 1;
16644 switch_expression->size[1] = iobj_12->Type->size[1];
16645 cartes_emxEnsureCapacity_char_T(switch_expression,
16646 cartesian_trajectory_planner_B.b_kstr_m);
16647 cartesian_trajectory_planner_B.loop_ub_o = iobj_12->Type->size[0] *
16648 iobj_12->Type->size[1] - 1;
16649 for (cartesian_trajectory_planner_B.b_kstr_m = 0;
16650 cartesian_trajectory_planner_B.b_kstr_m <=
16651 cartesian_trajectory_planner_B.loop_ub_o;
16652 cartesian_trajectory_planner_B.b_kstr_m++) {
16653 switch_expression->data[cartesian_trajectory_planner_B.b_kstr_m] =
16654 iobj_12->Type->data[cartesian_trajectory_planner_B.b_kstr_m];
16655 }
16656
16657 cartesian_trajectory_planner_B.b_bool_pn = false;
16658 if (switch_expression->size[1] == 8) {
16659 cartesian_trajectory_planner_B.b_kstr_m = 1;
16660 do {
16661 exitg1 = 0;
16662 if (cartesian_trajectory_planner_B.b_kstr_m - 1 < 8) {
16663 cartesian_trajectory_planner_B.loop_ub_o =
16664 cartesian_trajectory_planner_B.b_kstr_m - 1;
16665 if (switch_expression->data[cartesian_trajectory_planner_B.loop_ub_o] !=
16666 cartesian_trajectory_planner_B.b_nj[cartesian_trajectory_planner_B.loop_ub_o])
16667 {
16668 exitg1 = 1;
16669 } else {
16670 cartesian_trajectory_planner_B.b_kstr_m++;
16671 }
16672 } else {
16673 cartesian_trajectory_planner_B.b_bool_pn = true;
16674 exitg1 = 1;
16675 }
16676 } while (exitg1 == 0);
16677 }
16678
16679 if (cartesian_trajectory_planner_B.b_bool_pn) {
16680 cartesian_trajectory_planner_B.b_kstr_m = 0;
16681 } else {
16682 for (cartesian_trajectory_planner_B.b_kstr_m = 0;
16683 cartesian_trajectory_planner_B.b_kstr_m < 9;
16684 cartesian_trajectory_planner_B.b_kstr_m++) {
16685 cartesian_trajectory_planner_B.b_g[cartesian_trajectory_planner_B.b_kstr_m]
16686 = tmp_4[cartesian_trajectory_planner_B.b_kstr_m];
16687 }
16688
16689 cartesian_trajectory_planner_B.b_bool_pn = false;
16690 if (switch_expression->size[1] == 9) {
16691 cartesian_trajectory_planner_B.b_kstr_m = 1;
16692 do {
16693 exitg1 = 0;
16694 if (cartesian_trajectory_planner_B.b_kstr_m - 1 < 9) {
16695 cartesian_trajectory_planner_B.loop_ub_o =
16696 cartesian_trajectory_planner_B.b_kstr_m - 1;
16697 if (switch_expression->data[cartesian_trajectory_planner_B.loop_ub_o]
16698 !=
16699 cartesian_trajectory_planner_B.b_g[cartesian_trajectory_planner_B.loop_ub_o])
16700 {
16701 exitg1 = 1;
16702 } else {
16703 cartesian_trajectory_planner_B.b_kstr_m++;
16704 }
16705 } else {
16706 cartesian_trajectory_planner_B.b_bool_pn = true;
16707 exitg1 = 1;
16708 }
16709 } while (exitg1 == 0);
16710 }
16711
16712 if (cartesian_trajectory_planner_B.b_bool_pn) {
16713 cartesian_trajectory_planner_B.b_kstr_m = 1;
16714 } else {
16715 cartesian_trajectory_planner_B.b_kstr_m = -1;
16716 }
16717 }
16718
16719 switch (cartesian_trajectory_planner_B.b_kstr_m) {
16720 case 0:
16721 cartesian_trajectory_planner_B.iv1[0] = 0;
16722 cartesian_trajectory_planner_B.iv1[1] = 0;
16723 cartesian_trajectory_planner_B.iv1[2] = 1;
16724 cartesian_trajectory_planner_B.iv1[3] = 0;
16725 cartesian_trajectory_planner_B.iv1[4] = 0;
16726 cartesian_trajectory_planner_B.iv1[5] = 0;
16727 for (cartesian_trajectory_planner_B.b_kstr_m = 0;
16728 cartesian_trajectory_planner_B.b_kstr_m < 6;
16729 cartesian_trajectory_planner_B.b_kstr_m++) {
16730 cartesian_trajectory_planner_B.msubspace_data_i[cartesian_trajectory_planner_B.b_kstr_m]
16731 =
16732 cartesian_trajectory_planner_B.iv1[cartesian_trajectory_planner_B.b_kstr_m];
16733 }
16734
16735 cartesian_trajectory_planner_B.poslim_data_a[0] = -3.1415926535897931;
16736 cartesian_trajectory_planner_B.poslim_data_a[1] = 3.1415926535897931;
16737 iobj_12->VelocityNumber = 1.0;
16738 iobj_12->PositionNumber = 1.0;
16739 iobj_12->JointAxisInternal[0] = 0.0;
16740 iobj_12->JointAxisInternal[1] = 0.0;
16741 iobj_12->JointAxisInternal[2] = 1.0;
16742 break;
16743
16744 case 1:
16745 cartesian_trajectory_planner_B.iv1[0] = 0;
16746 cartesian_trajectory_planner_B.iv1[1] = 0;
16747 cartesian_trajectory_planner_B.iv1[2] = 0;
16748 cartesian_trajectory_planner_B.iv1[3] = 0;
16749 cartesian_trajectory_planner_B.iv1[4] = 0;
16750 cartesian_trajectory_planner_B.iv1[5] = 1;
16751 for (cartesian_trajectory_planner_B.b_kstr_m = 0;
16752 cartesian_trajectory_planner_B.b_kstr_m < 6;
16753 cartesian_trajectory_planner_B.b_kstr_m++) {
16754 cartesian_trajectory_planner_B.msubspace_data_i[cartesian_trajectory_planner_B.b_kstr_m]
16755 =
16756 cartesian_trajectory_planner_B.iv1[cartesian_trajectory_planner_B.b_kstr_m];
16757 }
16758
16759 cartesian_trajectory_planner_B.poslim_data_a[0] = -0.5;
16760 cartesian_trajectory_planner_B.poslim_data_a[1] = 0.5;
16761 iobj_12->VelocityNumber = 1.0;
16762 iobj_12->PositionNumber = 1.0;
16763 iobj_12->JointAxisInternal[0] = 0.0;
16764 iobj_12->JointAxisInternal[1] = 0.0;
16765 iobj_12->JointAxisInternal[2] = 1.0;
16766 break;
16767
16768 default:
16769 for (cartesian_trajectory_planner_B.b_kstr_m = 0;
16770 cartesian_trajectory_planner_B.b_kstr_m < 6;
16771 cartesian_trajectory_planner_B.b_kstr_m++) {
16772 cartesian_trajectory_planner_B.msubspace_data_i[cartesian_trajectory_planner_B.b_kstr_m]
16773 = 0;
16774 }
16775
16776 cartesian_trajectory_planner_B.poslim_data_a[0] = 0.0;
16777 cartesian_trajectory_planner_B.poslim_data_a[1] = 0.0;
16778 iobj_12->VelocityNumber = 0.0;
16779 iobj_12->PositionNumber = 0.0;
16780 iobj_12->JointAxisInternal[0] = 0.0;
16781 iobj_12->JointAxisInternal[1] = 0.0;
16782 iobj_12->JointAxisInternal[2] = 0.0;
16783 break;
16784 }
16785
16786 cartesian_trajectory_planner_B.b_kstr_m = iobj_12->MotionSubspace->size[0] *
16787 iobj_12->MotionSubspace->size[1];
16788 iobj_12->MotionSubspace->size[0] = 6;
16789 iobj_12->MotionSubspace->size[1] = 1;
16790 cartes_emxEnsureCapacity_real_T(iobj_12->MotionSubspace,
16791 cartesian_trajectory_planner_B.b_kstr_m);
16792 for (cartesian_trajectory_planner_B.b_kstr_m = 0;
16793 cartesian_trajectory_planner_B.b_kstr_m < 6;
16794 cartesian_trajectory_planner_B.b_kstr_m++) {
16795 iobj_12->MotionSubspace->data[cartesian_trajectory_planner_B.b_kstr_m] =
16796 cartesian_trajectory_planner_B.msubspace_data_i[cartesian_trajectory_planner_B.b_kstr_m];
16797 }
16798
16799 cartesian_trajectory_planner_B.b_kstr_m = iobj_12->
16800 PositionLimitsInternal->size[0] * iobj_12->PositionLimitsInternal->size[1];
16801 iobj_12->PositionLimitsInternal->size[0] = 1;
16802 iobj_12->PositionLimitsInternal->size[1] = 2;
16803 cartes_emxEnsureCapacity_real_T(iobj_12->PositionLimitsInternal,
16804 cartesian_trajectory_planner_B.b_kstr_m);
16805 for (cartesian_trajectory_planner_B.b_kstr_m = 0;
16806 cartesian_trajectory_planner_B.b_kstr_m < 2;
16807 cartesian_trajectory_planner_B.b_kstr_m++) {
16808 iobj_12->PositionLimitsInternal->
16809 data[cartesian_trajectory_planner_B.b_kstr_m] =
16810 cartesian_trajectory_planner_B.poslim_data_a[cartesian_trajectory_planner_B.b_kstr_m];
16811 }
16812
16813 cartesian_trajectory_planner_B.b_kstr_m = iobj_12->HomePositionInternal->size
16814 [0];
16815 iobj_12->HomePositionInternal->size[0] = 1;
16816 cartes_emxEnsureCapacity_real_T(iobj_12->HomePositionInternal,
16817 cartesian_trajectory_planner_B.b_kstr_m);
16818 for (cartesian_trajectory_planner_B.b_kstr_m = 0;
16819 cartesian_trajectory_planner_B.b_kstr_m < 1;
16820 cartesian_trajectory_planner_B.b_kstr_m++) {
16821 iobj_12->HomePositionInternal->data[0] = 0.0;
16822 }
16823
16824 iobj_5->JointInternal = iobj_12;
16825 iobj_5->Index = -1.0;
16826 iobj_5->ParentIndex = -1.0;
16827 obj->Bodies[6] = iobj_5;
16828 cartesian_trajectory_planner_B.b_kstr_m = iobj_6->NameInternal->size[0] *
16829 iobj_6->NameInternal->size[1];
16830 iobj_6->NameInternal->size[0] = 1;
16831 iobj_6->NameInternal->size[1] = 10;
16832 cartes_emxEnsureCapacity_char_T(iobj_6->NameInternal,
16833 cartesian_trajectory_planner_B.b_kstr_m);
16834 for (cartesian_trajectory_planner_B.b_kstr_m = 0;
16835 cartesian_trajectory_planner_B.b_kstr_m < 10;
16836 cartesian_trajectory_planner_B.b_kstr_m++) {
16837 iobj_6->NameInternal->data[cartesian_trajectory_planner_B.b_kstr_m] =
16838 tmp_7[cartesian_trajectory_planner_B.b_kstr_m];
16839 }
16840
16841 iobj_13->InTree = false;
16842 for (cartesian_trajectory_planner_B.b_kstr_m = 0;
16843 cartesian_trajectory_planner_B.b_kstr_m < 16;
16844 cartesian_trajectory_planner_B.b_kstr_m++) {
16845 iobj_13->JointToParentTransform[cartesian_trajectory_planner_B.b_kstr_m] =
16846 tmp_0[cartesian_trajectory_planner_B.b_kstr_m];
16847 }
16848
16849 for (cartesian_trajectory_planner_B.b_kstr_m = 0;
16850 cartesian_trajectory_planner_B.b_kstr_m < 16;
16851 cartesian_trajectory_planner_B.b_kstr_m++) {
16852 iobj_13->ChildToJointTransform[cartesian_trajectory_planner_B.b_kstr_m] =
16853 tmp_0[cartesian_trajectory_planner_B.b_kstr_m];
16854 }
16855
16856 cartesian_trajectory_planner_B.b_kstr_m = iobj_13->NameInternal->size[0] *
16857 iobj_13->NameInternal->size[1];
16858 iobj_13->NameInternal->size[0] = 1;
16859 iobj_13->NameInternal->size[1] = 14;
16860 cartes_emxEnsureCapacity_char_T(iobj_13->NameInternal,
16861 cartesian_trajectory_planner_B.b_kstr_m);
16862 for (cartesian_trajectory_planner_B.b_kstr_m = 0;
16863 cartesian_trajectory_planner_B.b_kstr_m < 14;
16864 cartesian_trajectory_planner_B.b_kstr_m++) {
16865 iobj_13->NameInternal->data[cartesian_trajectory_planner_B.b_kstr_m] =
16866 tmp_8[cartesian_trajectory_planner_B.b_kstr_m];
16867 }
16868
16869 cartesian_trajectory_planner_B.b_kstr_m = iobj_13->Type->size[0] *
16870 iobj_13->Type->size[1];
16871 iobj_13->Type->size[0] = 1;
16872 iobj_13->Type->size[1] = 5;
16873 cartes_emxEnsureCapacity_char_T(iobj_13->Type,
16874 cartesian_trajectory_planner_B.b_kstr_m);
16875 for (cartesian_trajectory_planner_B.b_kstr_m = 0;
16876 cartesian_trajectory_planner_B.b_kstr_m < 5;
16877 cartesian_trajectory_planner_B.b_kstr_m++) {
16878 iobj_13->Type->data[cartesian_trajectory_planner_B.b_kstr_m] =
16879 tmp_2[cartesian_trajectory_planner_B.b_kstr_m];
16880 }
16881
16882 cartesian_trajectory_planner_B.b_kstr_m = switch_expression->size[0] *
16883 switch_expression->size[1];
16884 switch_expression->size[0] = 1;
16885 switch_expression->size[1] = iobj_13->Type->size[1];
16886 cartes_emxEnsureCapacity_char_T(switch_expression,
16887 cartesian_trajectory_planner_B.b_kstr_m);
16888 cartesian_trajectory_planner_B.loop_ub_o = iobj_13->Type->size[0] *
16889 iobj_13->Type->size[1] - 1;
16890 for (cartesian_trajectory_planner_B.b_kstr_m = 0;
16891 cartesian_trajectory_planner_B.b_kstr_m <=
16892 cartesian_trajectory_planner_B.loop_ub_o;
16893 cartesian_trajectory_planner_B.b_kstr_m++) {
16894 switch_expression->data[cartesian_trajectory_planner_B.b_kstr_m] =
16895 iobj_13->Type->data[cartesian_trajectory_planner_B.b_kstr_m];
16896 }
16897
16898 cartesian_trajectory_planner_B.b_bool_pn = false;
16899 if (switch_expression->size[1] == 8) {
16900 cartesian_trajectory_planner_B.b_kstr_m = 1;
16901 do {
16902 exitg1 = 0;
16903 if (cartesian_trajectory_planner_B.b_kstr_m - 1 < 8) {
16904 cartesian_trajectory_planner_B.loop_ub_o =
16905 cartesian_trajectory_planner_B.b_kstr_m - 1;
16906 if (switch_expression->data[cartesian_trajectory_planner_B.loop_ub_o] !=
16907 cartesian_trajectory_planner_B.b_nj[cartesian_trajectory_planner_B.loop_ub_o])
16908 {
16909 exitg1 = 1;
16910 } else {
16911 cartesian_trajectory_planner_B.b_kstr_m++;
16912 }
16913 } else {
16914 cartesian_trajectory_planner_B.b_bool_pn = true;
16915 exitg1 = 1;
16916 }
16917 } while (exitg1 == 0);
16918 }
16919
16920 if (cartesian_trajectory_planner_B.b_bool_pn) {
16921 cartesian_trajectory_planner_B.b_kstr_m = 0;
16922 } else {
16923 for (cartesian_trajectory_planner_B.b_kstr_m = 0;
16924 cartesian_trajectory_planner_B.b_kstr_m < 9;
16925 cartesian_trajectory_planner_B.b_kstr_m++) {
16926 cartesian_trajectory_planner_B.b_g[cartesian_trajectory_planner_B.b_kstr_m]
16927 = tmp_4[cartesian_trajectory_planner_B.b_kstr_m];
16928 }
16929
16930 cartesian_trajectory_planner_B.b_bool_pn = false;
16931 if (switch_expression->size[1] == 9) {
16932 cartesian_trajectory_planner_B.b_kstr_m = 1;
16933 do {
16934 exitg1 = 0;
16935 if (cartesian_trajectory_planner_B.b_kstr_m - 1 < 9) {
16936 cartesian_trajectory_planner_B.loop_ub_o =
16937 cartesian_trajectory_planner_B.b_kstr_m - 1;
16938 if (switch_expression->data[cartesian_trajectory_planner_B.loop_ub_o]
16939 !=
16940 cartesian_trajectory_planner_B.b_g[cartesian_trajectory_planner_B.loop_ub_o])
16941 {
16942 exitg1 = 1;
16943 } else {
16944 cartesian_trajectory_planner_B.b_kstr_m++;
16945 }
16946 } else {
16947 cartesian_trajectory_planner_B.b_bool_pn = true;
16948 exitg1 = 1;
16949 }
16950 } while (exitg1 == 0);
16951 }
16952
16953 if (cartesian_trajectory_planner_B.b_bool_pn) {
16954 cartesian_trajectory_planner_B.b_kstr_m = 1;
16955 } else {
16956 cartesian_trajectory_planner_B.b_kstr_m = -1;
16957 }
16958 }
16959
16960 cartesian_trajec_emxFree_char_T(&switch_expression);
16961 switch (cartesian_trajectory_planner_B.b_kstr_m) {
16962 case 0:
16963 cartesian_trajectory_planner_B.iv1[0] = 0;
16964 cartesian_trajectory_planner_B.iv1[1] = 0;
16965 cartesian_trajectory_planner_B.iv1[2] = 1;
16966 cartesian_trajectory_planner_B.iv1[3] = 0;
16967 cartesian_trajectory_planner_B.iv1[4] = 0;
16968 cartesian_trajectory_planner_B.iv1[5] = 0;
16969 for (cartesian_trajectory_planner_B.b_kstr_m = 0;
16970 cartesian_trajectory_planner_B.b_kstr_m < 6;
16971 cartesian_trajectory_planner_B.b_kstr_m++) {
16972 cartesian_trajectory_planner_B.msubspace_data_i[cartesian_trajectory_planner_B.b_kstr_m]
16973 =
16974 cartesian_trajectory_planner_B.iv1[cartesian_trajectory_planner_B.b_kstr_m];
16975 }
16976
16977 cartesian_trajectory_planner_B.poslim_data_a[0] = -3.1415926535897931;
16978 cartesian_trajectory_planner_B.poslim_data_a[1] = 3.1415926535897931;
16979 iobj_13->VelocityNumber = 1.0;
16980 iobj_13->PositionNumber = 1.0;
16981 iobj_13->JointAxisInternal[0] = 0.0;
16982 iobj_13->JointAxisInternal[1] = 0.0;
16983 iobj_13->JointAxisInternal[2] = 1.0;
16984 break;
16985
16986 case 1:
16987 cartesian_trajectory_planner_B.iv1[0] = 0;
16988 cartesian_trajectory_planner_B.iv1[1] = 0;
16989 cartesian_trajectory_planner_B.iv1[2] = 0;
16990 cartesian_trajectory_planner_B.iv1[3] = 0;
16991 cartesian_trajectory_planner_B.iv1[4] = 0;
16992 cartesian_trajectory_planner_B.iv1[5] = 1;
16993 for (cartesian_trajectory_planner_B.b_kstr_m = 0;
16994 cartesian_trajectory_planner_B.b_kstr_m < 6;
16995 cartesian_trajectory_planner_B.b_kstr_m++) {
16996 cartesian_trajectory_planner_B.msubspace_data_i[cartesian_trajectory_planner_B.b_kstr_m]
16997 =
16998 cartesian_trajectory_planner_B.iv1[cartesian_trajectory_planner_B.b_kstr_m];
16999 }
17000
17001 cartesian_trajectory_planner_B.poslim_data_a[0] = -0.5;
17002 cartesian_trajectory_planner_B.poslim_data_a[1] = 0.5;
17003 iobj_13->VelocityNumber = 1.0;
17004 iobj_13->PositionNumber = 1.0;
17005 iobj_13->JointAxisInternal[0] = 0.0;
17006 iobj_13->JointAxisInternal[1] = 0.0;
17007 iobj_13->JointAxisInternal[2] = 1.0;
17008 break;
17009
17010 default:
17011 for (cartesian_trajectory_planner_B.b_kstr_m = 0;
17012 cartesian_trajectory_planner_B.b_kstr_m < 6;
17013 cartesian_trajectory_planner_B.b_kstr_m++) {
17014 cartesian_trajectory_planner_B.msubspace_data_i[cartesian_trajectory_planner_B.b_kstr_m]
17015 = 0;
17016 }
17017
17018 cartesian_trajectory_planner_B.poslim_data_a[0] = 0.0;
17019 cartesian_trajectory_planner_B.poslim_data_a[1] = 0.0;
17020 iobj_13->VelocityNumber = 0.0;
17021 iobj_13->PositionNumber = 0.0;
17022 iobj_13->JointAxisInternal[0] = 0.0;
17023 iobj_13->JointAxisInternal[1] = 0.0;
17024 iobj_13->JointAxisInternal[2] = 0.0;
17025 break;
17026 }
17027
17028 cartesian_trajectory_planner_B.b_kstr_m = iobj_13->MotionSubspace->size[0] *
17029 iobj_13->MotionSubspace->size[1];
17030 iobj_13->MotionSubspace->size[0] = 6;
17031 iobj_13->MotionSubspace->size[1] = 1;
17032 cartes_emxEnsureCapacity_real_T(iobj_13->MotionSubspace,
17033 cartesian_trajectory_planner_B.b_kstr_m);
17034 for (cartesian_trajectory_planner_B.b_kstr_m = 0;
17035 cartesian_trajectory_planner_B.b_kstr_m < 6;
17036 cartesian_trajectory_planner_B.b_kstr_m++) {
17037 iobj_13->MotionSubspace->data[cartesian_trajectory_planner_B.b_kstr_m] =
17038 cartesian_trajectory_planner_B.msubspace_data_i[cartesian_trajectory_planner_B.b_kstr_m];
17039 }
17040
17041 cartesian_trajectory_planner_B.b_kstr_m = iobj_13->
17042 PositionLimitsInternal->size[0] * iobj_13->PositionLimitsInternal->size[1];
17043 iobj_13->PositionLimitsInternal->size[0] = 1;
17044 iobj_13->PositionLimitsInternal->size[1] = 2;
17045 cartes_emxEnsureCapacity_real_T(iobj_13->PositionLimitsInternal,
17046 cartesian_trajectory_planner_B.b_kstr_m);
17047 for (cartesian_trajectory_planner_B.b_kstr_m = 0;
17048 cartesian_trajectory_planner_B.b_kstr_m < 2;
17049 cartesian_trajectory_planner_B.b_kstr_m++) {
17050 iobj_13->PositionLimitsInternal->
17051 data[cartesian_trajectory_planner_B.b_kstr_m] =
17052 cartesian_trajectory_planner_B.poslim_data_a[cartesian_trajectory_planner_B.b_kstr_m];
17053 }
17054
17055 cartesian_trajectory_planner_B.b_kstr_m = iobj_13->HomePositionInternal->size
17056 [0];
17057 iobj_13->HomePositionInternal->size[0] = 1;
17058 cartes_emxEnsureCapacity_real_T(iobj_13->HomePositionInternal,
17059 cartesian_trajectory_planner_B.b_kstr_m);
17060 for (cartesian_trajectory_planner_B.b_kstr_m = 0;
17061 cartesian_trajectory_planner_B.b_kstr_m < 1;
17062 cartesian_trajectory_planner_B.b_kstr_m++) {
17063 iobj_13->HomePositionInternal->data[0] = 0.0;
17064 }
17065
17066 iobj_6->JointInternal = iobj_13;
17067 iobj_6->Index = -1.0;
17068 iobj_6->ParentIndex = -1.0;
17069 obj->Bodies[7] = iobj_6;
17070 obj->NumBodies = 0.0;
17071 obj->NumNonFixedBodies = 0.0;
17072 obj->PositionNumber = 0.0;
17073 obj->VelocityNumber = 0.0;
17074 cartesian_trajectory_plann_rand(cartesian_trajectory_planner_B.unusedExpr_k);
17075 for (cartesian_trajectory_planner_B.b_kstr_m = 0;
17076 cartesian_trajectory_planner_B.b_kstr_m < 8;
17077 cartesian_trajectory_planner_B.b_kstr_m++) {
17078 obj->PositionDoFMap[cartesian_trajectory_planner_B.b_kstr_m] = 0.0;
17079 }
17080
17081 for (cartesian_trajectory_planner_B.b_kstr_m = 0;
17082 cartesian_trajectory_planner_B.b_kstr_m < 8;
17083 cartesian_trajectory_planner_B.b_kstr_m++) {
17084 obj->PositionDoFMap[cartesian_trajectory_planner_B.b_kstr_m + 8] = -1.0;
17085 }
17086
17087 for (cartesian_trajectory_planner_B.b_kstr_m = 0;
17088 cartesian_trajectory_planner_B.b_kstr_m < 8;
17089 cartesian_trajectory_planner_B.b_kstr_m++) {
17090 obj->VelocityDoFMap[cartesian_trajectory_planner_B.b_kstr_m] = 0.0;
17091 }
17092
17093 for (cartesian_trajectory_planner_B.b_kstr_m = 0;
17094 cartesian_trajectory_planner_B.b_kstr_m < 8;
17095 cartesian_trajectory_planner_B.b_kstr_m++) {
17096 obj->VelocityDoFMap[cartesian_trajectory_planner_B.b_kstr_m + 8] = -1.0;
17097 }
17098}
17099
17100static x_robotics_manip_internal_Rig_T *RigidBodyTree_RigidBodyTree_as
17101 (x_robotics_manip_internal_Rig_T *obj, w_robotics_manip_internal_Rig_T *iobj_0,
17102 w_robotics_manip_internal_Rig_T *iobj_1, w_robotics_manip_internal_Rig_T
17103 *iobj_2, w_robotics_manip_internal_Rig_T *iobj_3,
17104 w_robotics_manip_internal_Rig_T *iobj_4, w_robotics_manip_internal_Rig_T
17105 *iobj_5, w_robotics_manip_internal_Rig_T *iobj_6,
17106 c_rigidBodyJoint_cartesian__a_T *iobj_7, c_rigidBodyJoint_cartesian__a_T
17107 *iobj_8, c_rigidBodyJoint_cartesian__a_T *iobj_9,
17108 c_rigidBodyJoint_cartesian__a_T *iobj_10, c_rigidBodyJoint_cartesian__a_T
17109 *iobj_11, c_rigidBodyJoint_cartesian__a_T *iobj_12,
17110 c_rigidBodyJoint_cartesian__a_T *iobj_13, c_rigidBodyJoint_cartesian__a_T
17111 *iobj_14, c_rigidBodyJoint_cartesian__a_T *iobj_15,
17112 w_robotics_manip_internal_Rig_T *iobj_16)
17113{
17114 x_robotics_manip_internal_Rig_T *b_obj;
17115 emxArray_char_T_cartesian_tra_T *switch_expression;
17116 static const int8_T tmp[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1
17117 };
17118
17119 static const char_T tmp_0[8] = { 'b', 'a', 's', 'e', '_', 'j', 'n', 't' };
17120
17121 static const char_T tmp_1[5] = { 'f', 'i', 'x', 'e', 'd' };
17122
17123 static const char_T tmp_2[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
17124
17125 static const char_T tmp_3[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
17126
17127 int32_T exitg1;
17128 b_obj = obj;
17129 cartesian_trajectory_planner_B.b_kstr_c = obj->Base.NameInternal->size[0] *
17130 obj->Base.NameInternal->size[1];
17131 obj->Base.NameInternal->size[0] = 1;
17132 obj->Base.NameInternal->size[1] = 4;
17133 cartes_emxEnsureCapacity_char_T(obj->Base.NameInternal,
17134 cartesian_trajectory_planner_B.b_kstr_c);
17135 obj->Base.NameInternal->data[0] = 'b';
17136 obj->Base.NameInternal->data[1] = 'a';
17137 obj->Base.NameInternal->data[2] = 's';
17138 obj->Base.NameInternal->data[3] = 'e';
17139 iobj_15->InTree = false;
17140 for (cartesian_trajectory_planner_B.b_kstr_c = 0;
17141 cartesian_trajectory_planner_B.b_kstr_c < 16;
17142 cartesian_trajectory_planner_B.b_kstr_c++) {
17143 iobj_15->JointToParentTransform[cartesian_trajectory_planner_B.b_kstr_c] =
17144 tmp[cartesian_trajectory_planner_B.b_kstr_c];
17145 }
17146
17147 for (cartesian_trajectory_planner_B.b_kstr_c = 0;
17148 cartesian_trajectory_planner_B.b_kstr_c < 16;
17149 cartesian_trajectory_planner_B.b_kstr_c++) {
17150 iobj_15->ChildToJointTransform[cartesian_trajectory_planner_B.b_kstr_c] =
17151 tmp[cartesian_trajectory_planner_B.b_kstr_c];
17152 }
17153
17154 cartesian_trajectory_planner_B.b_kstr_c = iobj_15->NameInternal->size[0] *
17155 iobj_15->NameInternal->size[1];
17156 iobj_15->NameInternal->size[0] = 1;
17157 iobj_15->NameInternal->size[1] = 8;
17158 cartes_emxEnsureCapacity_char_T(iobj_15->NameInternal,
17159 cartesian_trajectory_planner_B.b_kstr_c);
17160 for (cartesian_trajectory_planner_B.b_kstr_c = 0;
17161 cartesian_trajectory_planner_B.b_kstr_c < 8;
17162 cartesian_trajectory_planner_B.b_kstr_c++) {
17163 iobj_15->NameInternal->data[cartesian_trajectory_planner_B.b_kstr_c] =
17164 tmp_0[cartesian_trajectory_planner_B.b_kstr_c];
17165 }
17166
17167 cartesian_trajectory_planner_B.b_kstr_c = iobj_15->Type->size[0] *
17168 iobj_15->Type->size[1];
17169 iobj_15->Type->size[0] = 1;
17170 iobj_15->Type->size[1] = 5;
17171 cartes_emxEnsureCapacity_char_T(iobj_15->Type,
17172 cartesian_trajectory_planner_B.b_kstr_c);
17173 for (cartesian_trajectory_planner_B.b_kstr_c = 0;
17174 cartesian_trajectory_planner_B.b_kstr_c < 5;
17175 cartesian_trajectory_planner_B.b_kstr_c++) {
17176 iobj_15->Type->data[cartesian_trajectory_planner_B.b_kstr_c] =
17177 tmp_1[cartesian_trajectory_planner_B.b_kstr_c];
17178 }
17179
17180 cartesian_trajec_emxInit_char_T(&switch_expression, 2);
17181 cartesian_trajectory_planner_B.b_kstr_c = switch_expression->size[0] *
17182 switch_expression->size[1];
17183 switch_expression->size[0] = 1;
17184 switch_expression->size[1] = iobj_15->Type->size[1];
17185 cartes_emxEnsureCapacity_char_T(switch_expression,
17186 cartesian_trajectory_planner_B.b_kstr_c);
17187 cartesian_trajectory_planner_B.loop_ub_e = iobj_15->Type->size[0] *
17188 iobj_15->Type->size[1] - 1;
17189 for (cartesian_trajectory_planner_B.b_kstr_c = 0;
17190 cartesian_trajectory_planner_B.b_kstr_c <=
17191 cartesian_trajectory_planner_B.loop_ub_e;
17192 cartesian_trajectory_planner_B.b_kstr_c++) {
17193 switch_expression->data[cartesian_trajectory_planner_B.b_kstr_c] =
17194 iobj_15->Type->data[cartesian_trajectory_planner_B.b_kstr_c];
17195 }
17196
17197 for (cartesian_trajectory_planner_B.b_kstr_c = 0;
17198 cartesian_trajectory_planner_B.b_kstr_c < 8;
17199 cartesian_trajectory_planner_B.b_kstr_c++) {
17200 cartesian_trajectory_planner_B.b_p2[cartesian_trajectory_planner_B.b_kstr_c]
17201 = tmp_2[cartesian_trajectory_planner_B.b_kstr_c];
17202 }
17203
17204 cartesian_trajectory_planner_B.b_bool_b = false;
17205 if (switch_expression->size[1] == 8) {
17206 cartesian_trajectory_planner_B.b_kstr_c = 1;
17207 do {
17208 exitg1 = 0;
17209 if (cartesian_trajectory_planner_B.b_kstr_c - 1 < 8) {
17210 cartesian_trajectory_planner_B.loop_ub_e =
17211 cartesian_trajectory_planner_B.b_kstr_c - 1;
17212 if (switch_expression->data[cartesian_trajectory_planner_B.loop_ub_e] !=
17213 cartesian_trajectory_planner_B.b_p2[cartesian_trajectory_planner_B.loop_ub_e])
17214 {
17215 exitg1 = 1;
17216 } else {
17217 cartesian_trajectory_planner_B.b_kstr_c++;
17218 }
17219 } else {
17220 cartesian_trajectory_planner_B.b_bool_b = true;
17221 exitg1 = 1;
17222 }
17223 } while (exitg1 == 0);
17224 }
17225
17226 if (cartesian_trajectory_planner_B.b_bool_b) {
17227 cartesian_trajectory_planner_B.b_kstr_c = 0;
17228 } else {
17229 for (cartesian_trajectory_planner_B.b_kstr_c = 0;
17230 cartesian_trajectory_planner_B.b_kstr_c < 9;
17231 cartesian_trajectory_planner_B.b_kstr_c++) {
17232 cartesian_trajectory_planner_B.b_a[cartesian_trajectory_planner_B.b_kstr_c]
17233 = tmp_3[cartesian_trajectory_planner_B.b_kstr_c];
17234 }
17235
17236 cartesian_trajectory_planner_B.b_bool_b = false;
17237 if (switch_expression->size[1] == 9) {
17238 cartesian_trajectory_planner_B.b_kstr_c = 1;
17239 do {
17240 exitg1 = 0;
17241 if (cartesian_trajectory_planner_B.b_kstr_c - 1 < 9) {
17242 cartesian_trajectory_planner_B.loop_ub_e =
17243 cartesian_trajectory_planner_B.b_kstr_c - 1;
17244 if (switch_expression->data[cartesian_trajectory_planner_B.loop_ub_e]
17245 !=
17246 cartesian_trajectory_planner_B.b_a[cartesian_trajectory_planner_B.loop_ub_e])
17247 {
17248 exitg1 = 1;
17249 } else {
17250 cartesian_trajectory_planner_B.b_kstr_c++;
17251 }
17252 } else {
17253 cartesian_trajectory_planner_B.b_bool_b = true;
17254 exitg1 = 1;
17255 }
17256 } while (exitg1 == 0);
17257 }
17258
17259 if (cartesian_trajectory_planner_B.b_bool_b) {
17260 cartesian_trajectory_planner_B.b_kstr_c = 1;
17261 } else {
17262 cartesian_trajectory_planner_B.b_kstr_c = -1;
17263 }
17264 }
17265
17266 cartesian_trajec_emxFree_char_T(&switch_expression);
17267 switch (cartesian_trajectory_planner_B.b_kstr_c) {
17268 case 0:
17269 cartesian_trajectory_planner_B.iv[0] = 0;
17270 cartesian_trajectory_planner_B.iv[1] = 0;
17271 cartesian_trajectory_planner_B.iv[2] = 1;
17272 cartesian_trajectory_planner_B.iv[3] = 0;
17273 cartesian_trajectory_planner_B.iv[4] = 0;
17274 cartesian_trajectory_planner_B.iv[5] = 0;
17275 for (cartesian_trajectory_planner_B.b_kstr_c = 0;
17276 cartesian_trajectory_planner_B.b_kstr_c < 6;
17277 cartesian_trajectory_planner_B.b_kstr_c++) {
17278 cartesian_trajectory_planner_B.msubspace_data[cartesian_trajectory_planner_B.b_kstr_c]
17279 =
17280 cartesian_trajectory_planner_B.iv[cartesian_trajectory_planner_B.b_kstr_c];
17281 }
17282
17283 cartesian_trajectory_planner_B.poslim_data[0] = -3.1415926535897931;
17284 cartesian_trajectory_planner_B.poslim_data[1] = 3.1415926535897931;
17285 iobj_15->VelocityNumber = 1.0;
17286 iobj_15->PositionNumber = 1.0;
17287 iobj_15->JointAxisInternal[0] = 0.0;
17288 iobj_15->JointAxisInternal[1] = 0.0;
17289 iobj_15->JointAxisInternal[2] = 1.0;
17290 break;
17291
17292 case 1:
17293 cartesian_trajectory_planner_B.iv[0] = 0;
17294 cartesian_trajectory_planner_B.iv[1] = 0;
17295 cartesian_trajectory_planner_B.iv[2] = 0;
17296 cartesian_trajectory_planner_B.iv[3] = 0;
17297 cartesian_trajectory_planner_B.iv[4] = 0;
17298 cartesian_trajectory_planner_B.iv[5] = 1;
17299 for (cartesian_trajectory_planner_B.b_kstr_c = 0;
17300 cartesian_trajectory_planner_B.b_kstr_c < 6;
17301 cartesian_trajectory_planner_B.b_kstr_c++) {
17302 cartesian_trajectory_planner_B.msubspace_data[cartesian_trajectory_planner_B.b_kstr_c]
17303 =
17304 cartesian_trajectory_planner_B.iv[cartesian_trajectory_planner_B.b_kstr_c];
17305 }
17306
17307 cartesian_trajectory_planner_B.poslim_data[0] = -0.5;
17308 cartesian_trajectory_planner_B.poslim_data[1] = 0.5;
17309 iobj_15->VelocityNumber = 1.0;
17310 iobj_15->PositionNumber = 1.0;
17311 iobj_15->JointAxisInternal[0] = 0.0;
17312 iobj_15->JointAxisInternal[1] = 0.0;
17313 iobj_15->JointAxisInternal[2] = 1.0;
17314 break;
17315
17316 default:
17317 for (cartesian_trajectory_planner_B.b_kstr_c = 0;
17318 cartesian_trajectory_planner_B.b_kstr_c < 6;
17319 cartesian_trajectory_planner_B.b_kstr_c++) {
17320 cartesian_trajectory_planner_B.msubspace_data[cartesian_trajectory_planner_B.b_kstr_c]
17321 = 0;
17322 }
17323
17324 cartesian_trajectory_planner_B.poslim_data[0] = 0.0;
17325 cartesian_trajectory_planner_B.poslim_data[1] = 0.0;
17326 iobj_15->VelocityNumber = 0.0;
17327 iobj_15->PositionNumber = 0.0;
17328 iobj_15->JointAxisInternal[0] = 0.0;
17329 iobj_15->JointAxisInternal[1] = 0.0;
17330 iobj_15->JointAxisInternal[2] = 0.0;
17331 break;
17332 }
17333
17334 cartesian_trajectory_planner_B.b_kstr_c = iobj_15->MotionSubspace->size[0] *
17335 iobj_15->MotionSubspace->size[1];
17336 iobj_15->MotionSubspace->size[0] = 6;
17337 iobj_15->MotionSubspace->size[1] = 1;
17338 cartes_emxEnsureCapacity_real_T(iobj_15->MotionSubspace,
17339 cartesian_trajectory_planner_B.b_kstr_c);
17340 for (cartesian_trajectory_planner_B.b_kstr_c = 0;
17341 cartesian_trajectory_planner_B.b_kstr_c < 6;
17342 cartesian_trajectory_planner_B.b_kstr_c++) {
17343 iobj_15->MotionSubspace->data[cartesian_trajectory_planner_B.b_kstr_c] =
17344 cartesian_trajectory_planner_B.msubspace_data[cartesian_trajectory_planner_B.b_kstr_c];
17345 }
17346
17347 cartesian_trajectory_planner_B.b_kstr_c = iobj_15->
17348 PositionLimitsInternal->size[0] * iobj_15->PositionLimitsInternal->size[1];
17349 iobj_15->PositionLimitsInternal->size[0] = 1;
17350 iobj_15->PositionLimitsInternal->size[1] = 2;
17351 cartes_emxEnsureCapacity_real_T(iobj_15->PositionLimitsInternal,
17352 cartesian_trajectory_planner_B.b_kstr_c);
17353 for (cartesian_trajectory_planner_B.b_kstr_c = 0;
17354 cartesian_trajectory_planner_B.b_kstr_c < 2;
17355 cartesian_trajectory_planner_B.b_kstr_c++) {
17356 iobj_15->PositionLimitsInternal->
17357 data[cartesian_trajectory_planner_B.b_kstr_c] =
17358 cartesian_trajectory_planner_B.poslim_data[cartesian_trajectory_planner_B.b_kstr_c];
17359 }
17360
17361 cartesian_trajectory_planner_B.b_kstr_c = iobj_15->HomePositionInternal->size
17362 [0];
17363 iobj_15->HomePositionInternal->size[0] = 1;
17364 cartes_emxEnsureCapacity_real_T(iobj_15->HomePositionInternal,
17365 cartesian_trajectory_planner_B.b_kstr_c);
17366 for (cartesian_trajectory_planner_B.b_kstr_c = 0;
17367 cartesian_trajectory_planner_B.b_kstr_c < 1;
17368 cartesian_trajectory_planner_B.b_kstr_c++) {
17369 iobj_15->HomePositionInternal->data[0] = 0.0;
17370 }
17371
17372 obj->Base.JointInternal = iobj_15;
17373 obj->Base.Index = -1.0;
17374 obj->Base.ParentIndex = -1.0;
17375 obj->Base.Index = 0.0;
17376 cartesian_trajectory_plann_rand(cartesian_trajectory_planner_B.unusedExpr);
17377 ca_RigidBodyTree_clearAllBodies(obj, iobj_0, iobj_1, iobj_2, iobj_3, iobj_4,
17378 iobj_5, iobj_6, iobj_8, iobj_9, iobj_10, iobj_11, iobj_12, iobj_13, iobj_14,
17379 iobj_7, iobj_16);
17380 return b_obj;
17381}
17382
17383static c_rigidBodyJoint_cartesian__a_T *c_rigidBodyJoint_rigidBodyJoint
17384 (c_rigidBodyJoint_cartesian__a_T *obj, const emxArray_char_T_cartesian_tra_T
17385 *jname, const emxArray_char_T_cartesian_tra_T *jtype)
17386{
17387 c_rigidBodyJoint_cartesian__a_T *b_obj;
17388 emxArray_char_T_cartesian_tra_T *switch_expression;
17389 static const int8_T tmp[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1
17390 };
17391
17392 static const char_T tmp_0[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
17393
17394 static const char_T tmp_1[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
17395
17396 static const char_T tmp_2[5] = { 'f', 'i', 'x', 'e', 'd' };
17397
17398 static const char_T tmp_3[128] = { '\x00', '\x01', '\x02', '\x03', '\x04',
17399 '\x05', '\x06', '\x07', '\x08', '\x09', '\x0a', '\x0b', '\x0c', '\x0d',
17400 '\x0e', '\x0f', '\x10', '\x11', '\x12', '\x13', '\x14', '\x15', '\x16',
17401 '\x17', '\x18', '\x19', '\x1a', '\x1b', '\x1c', '\x1d', '\x1e', '\x1f', ' ',
17402 '!', '\"', '#', '$', '%', '&', '\'', '(', ')', '*', '+', ',', '-', '.', '/',
17403 '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', ':', ';', '<', '=', '>',
17404 '?', '@', 'a', 'b', 'c', 'd', 'e', 'f', 'g', 'h', 'i', 'j', 'k', 'l', 'm',
17405 'n', 'o', 'p', 'q', 'r', 's', 't', 'u', 'v', 'w', 'x', 'y', 'z', '[', '\\',
17406 ']', '^', '_', '`', 'a', 'b', 'c', 'd', 'e', 'f', 'g', 'h', 'i', 'j', 'k',
17407 'l', 'm', 'n', 'o', 'p', 'q', 'r', 's', 't', 'u', 'v', 'w', 'x', 'y', 'z',
17408 '{', '|', '}', '~', '\x7f' };
17409
17410 boolean_T guard1 = false;
17411 boolean_T guard2 = false;
17412 boolean_T guard3 = false;
17413 boolean_T guard4 = false;
17414 int32_T exitg1;
17415 boolean_T guard11 = false;
17416 obj->InTree = false;
17417 for (cartesian_trajectory_planner_B.minnanb = 0;
17418 cartesian_trajectory_planner_B.minnanb < 16;
17419 cartesian_trajectory_planner_B.minnanb++) {
17420 obj->JointToParentTransform[cartesian_trajectory_planner_B.minnanb] =
17421 tmp[cartesian_trajectory_planner_B.minnanb];
17422 }
17423
17424 for (cartesian_trajectory_planner_B.minnanb = 0;
17425 cartesian_trajectory_planner_B.minnanb < 16;
17426 cartesian_trajectory_planner_B.minnanb++) {
17427 obj->ChildToJointTransform[cartesian_trajectory_planner_B.minnanb] =
17428 tmp[cartesian_trajectory_planner_B.minnanb];
17429 }
17430
17431 b_obj = obj;
17432 cartesian_trajectory_planner_B.minnanb = obj->NameInternal->size[0] *
17433 obj->NameInternal->size[1];
17434 obj->NameInternal->size[0] = 1;
17435 obj->NameInternal->size[1] = jname->size[1];
17436 cartes_emxEnsureCapacity_char_T(obj->NameInternal,
17437 cartesian_trajectory_planner_B.minnanb);
17438 cartesian_trajectory_planner_B.loop_ub_g0 = jname->size[0] * jname->size[1] -
17439 1;
17440 for (cartesian_trajectory_planner_B.minnanb = 0;
17441 cartesian_trajectory_planner_B.minnanb <=
17442 cartesian_trajectory_planner_B.loop_ub_g0;
17443 cartesian_trajectory_planner_B.minnanb++) {
17444 obj->NameInternal->data[cartesian_trajectory_planner_B.minnanb] =
17445 jname->data[cartesian_trajectory_planner_B.minnanb];
17446 }
17447
17448 cartesian_trajectory_planner_B.partial_match_size_idx_1 = 0;
17449 for (cartesian_trajectory_planner_B.minnanb = 0;
17450 cartesian_trajectory_planner_B.minnanb < 8;
17451 cartesian_trajectory_planner_B.minnanb++) {
17452 cartesian_trajectory_planner_B.vstr[cartesian_trajectory_planner_B.minnanb] =
17453 tmp_0[cartesian_trajectory_planner_B.minnanb];
17454 }
17455
17456 guard1 = false;
17457 guard2 = false;
17458 guard3 = false;
17459 guard4 = false;
17460 if (jtype->size[1] <= 8) {
17461 cartesian_trajectory_planner_B.loop_ub_g0 = jtype->size[1];
17462 for (cartesian_trajectory_planner_B.minnanb = 0;
17463 cartesian_trajectory_planner_B.minnanb < 8;
17464 cartesian_trajectory_planner_B.minnanb++) {
17465 cartesian_trajectory_planner_B.b_cq[cartesian_trajectory_planner_B.minnanb]
17466 = tmp_0[cartesian_trajectory_planner_B.minnanb];
17467 }
17468
17469 cartesian_trajectory_planner_B.b_bool_cg = false;
17470 cartesian_trajectory_planner_B.minnanb = jtype->size[1];
17471 if (cartesian_trajectory_planner_B.minnanb >= 8) {
17472 cartesian_trajectory_planner_B.minnanb = 8;
17473 }
17474
17475 guard11 = false;
17476 if (cartesian_trajectory_planner_B.loop_ub_g0 <=
17477 cartesian_trajectory_planner_B.minnanb) {
17478 if (cartesian_trajectory_planner_B.minnanb <
17479 cartesian_trajectory_planner_B.loop_ub_g0) {
17480 cartesian_trajectory_planner_B.loop_ub_g0 =
17481 cartesian_trajectory_planner_B.minnanb;
17482 }
17483
17484 cartesian_trajectory_planner_B.minnanb =
17485 cartesian_trajectory_planner_B.loop_ub_g0 - 1;
17486 guard11 = true;
17487 } else {
17488 if (jtype->size[1] == 8) {
17489 cartesian_trajectory_planner_B.minnanb = 7;
17490 guard11 = true;
17491 }
17492 }
17493
17494 if (guard11) {
17495 cartesian_trajectory_planner_B.loop_ub_g0 = 1;
17496 do {
17497 exitg1 = 0;
17498 if (cartesian_trajectory_planner_B.loop_ub_g0 - 1 <=
17499 cartesian_trajectory_planner_B.minnanb) {
17500 cartesian_trajectory_planner_B.kstr_c =
17501 cartesian_trajectory_planner_B.loop_ub_g0 - 1;
17502 if (tmp_3[static_cast<uint8_T>(jtype->
17503 data[cartesian_trajectory_planner_B.kstr_c]) & 127] != tmp_3[
17504 static_cast<int32_T>
17505 (cartesian_trajectory_planner_B.b_cq[cartesian_trajectory_planner_B.kstr_c])])
17506 {
17507 exitg1 = 1;
17508 } else {
17509 cartesian_trajectory_planner_B.loop_ub_g0++;
17510 }
17511 } else {
17512 cartesian_trajectory_planner_B.b_bool_cg = true;
17513 exitg1 = 1;
17514 }
17515 } while (exitg1 == 0);
17516 }
17517
17518 if (cartesian_trajectory_planner_B.b_bool_cg) {
17519 if (jtype->size[1] == 8) {
17520 cartesian_trajectory_planner_B.nmatched = 1;
17521 cartesian_trajectory_planner_B.partial_match_size_idx_1 = 8;
17522 for (cartesian_trajectory_planner_B.minnanb = 0;
17523 cartesian_trajectory_planner_B.minnanb < 8;
17524 cartesian_trajectory_planner_B.minnanb++) {
17525 cartesian_trajectory_planner_B.b_na[cartesian_trajectory_planner_B.minnanb]
17526 =
17527 cartesian_trajectory_planner_B.vstr[cartesian_trajectory_planner_B.minnanb];
17528 }
17529 } else {
17530 cartesian_trajectory_planner_B.partial_match_size_idx_1 = 8;
17531 for (cartesian_trajectory_planner_B.minnanb = 0;
17532 cartesian_trajectory_planner_B.minnanb < 8;
17533 cartesian_trajectory_planner_B.minnanb++) {
17534 cartesian_trajectory_planner_B.partial_match_data[cartesian_trajectory_planner_B.minnanb]
17535 =
17536 cartesian_trajectory_planner_B.vstr[cartesian_trajectory_planner_B.minnanb];
17537 }
17538
17539 cartesian_trajectory_planner_B.matched = true;
17540 cartesian_trajectory_planner_B.nmatched = 1;
17541 guard3 = true;
17542 }
17543 } else {
17544 guard4 = true;
17545 }
17546 } else {
17547 guard4 = true;
17548 }
17549
17550 if (guard4) {
17551 cartesian_trajectory_planner_B.matched = false;
17552 cartesian_trajectory_planner_B.nmatched = 0;
17553 guard3 = true;
17554 }
17555
17556 if (guard3) {
17557 for (cartesian_trajectory_planner_B.minnanb = 0;
17558 cartesian_trajectory_planner_B.minnanb < 9;
17559 cartesian_trajectory_planner_B.minnanb++) {
17560 cartesian_trajectory_planner_B.b_vstr[cartesian_trajectory_planner_B.minnanb]
17561 = tmp_1[cartesian_trajectory_planner_B.minnanb];
17562 }
17563
17564 if (jtype->size[1] <= 9) {
17565 cartesian_trajectory_planner_B.loop_ub_g0 = jtype->size[1];
17566 for (cartesian_trajectory_planner_B.minnanb = 0;
17567 cartesian_trajectory_planner_B.minnanb < 9;
17568 cartesian_trajectory_planner_B.minnanb++) {
17569 cartesian_trajectory_planner_B.b_na[cartesian_trajectory_planner_B.minnanb]
17570 = tmp_1[cartesian_trajectory_planner_B.minnanb];
17571 }
17572
17573 cartesian_trajectory_planner_B.b_bool_cg = false;
17574 cartesian_trajectory_planner_B.minnanb = jtype->size[1];
17575 if (cartesian_trajectory_planner_B.minnanb >= 9) {
17576 cartesian_trajectory_planner_B.minnanb = 9;
17577 }
17578
17579 guard11 = false;
17580 if (cartesian_trajectory_planner_B.loop_ub_g0 <=
17581 cartesian_trajectory_planner_B.minnanb) {
17582 if (cartesian_trajectory_planner_B.minnanb <
17583 cartesian_trajectory_planner_B.loop_ub_g0) {
17584 cartesian_trajectory_planner_B.loop_ub_g0 =
17585 cartesian_trajectory_planner_B.minnanb;
17586 }
17587
17588 cartesian_trajectory_planner_B.minnanb =
17589 cartesian_trajectory_planner_B.loop_ub_g0 - 1;
17590 guard11 = true;
17591 } else {
17592 if (jtype->size[1] == 9) {
17593 cartesian_trajectory_planner_B.minnanb = 8;
17594 guard11 = true;
17595 }
17596 }
17597
17598 if (guard11) {
17599 cartesian_trajectory_planner_B.loop_ub_g0 = 1;
17600 do {
17601 exitg1 = 0;
17602 if (cartesian_trajectory_planner_B.loop_ub_g0 - 1 <=
17603 cartesian_trajectory_planner_B.minnanb) {
17604 cartesian_trajectory_planner_B.kstr_c =
17605 cartesian_trajectory_planner_B.loop_ub_g0 - 1;
17606 if (tmp_3[static_cast<uint8_T>(jtype->
17607 data[cartesian_trajectory_planner_B.kstr_c]) & 127] != tmp_3[
17608 static_cast<int32_T>
17609 (cartesian_trajectory_planner_B.b_na[cartesian_trajectory_planner_B.kstr_c])])
17610 {
17611 exitg1 = 1;
17612 } else {
17613 cartesian_trajectory_planner_B.loop_ub_g0++;
17614 }
17615 } else {
17616 cartesian_trajectory_planner_B.b_bool_cg = true;
17617 exitg1 = 1;
17618 }
17619 } while (exitg1 == 0);
17620 }
17621
17622 if (cartesian_trajectory_planner_B.b_bool_cg) {
17623 if (jtype->size[1] == 9) {
17624 cartesian_trajectory_planner_B.nmatched = 1;
17625 cartesian_trajectory_planner_B.partial_match_size_idx_1 = 9;
17626 for (cartesian_trajectory_planner_B.minnanb = 0;
17627 cartesian_trajectory_planner_B.minnanb < 9;
17628 cartesian_trajectory_planner_B.minnanb++) {
17629 cartesian_trajectory_planner_B.b_na[cartesian_trajectory_planner_B.minnanb]
17630 =
17631 cartesian_trajectory_planner_B.b_vstr[cartesian_trajectory_planner_B.minnanb];
17632 }
17633 } else {
17634 if (!cartesian_trajectory_planner_B.matched) {
17635 cartesian_trajectory_planner_B.partial_match_size_idx_1 = 9;
17636 for (cartesian_trajectory_planner_B.minnanb = 0;
17637 cartesian_trajectory_planner_B.minnanb < 9;
17638 cartesian_trajectory_planner_B.minnanb++) {
17639 cartesian_trajectory_planner_B.partial_match_data[cartesian_trajectory_planner_B.minnanb]
17640 =
17641 cartesian_trajectory_planner_B.b_vstr[cartesian_trajectory_planner_B.minnanb];
17642 }
17643 }
17644
17645 cartesian_trajectory_planner_B.matched = true;
17646 cartesian_trajectory_planner_B.nmatched++;
17647 guard2 = true;
17648 }
17649 } else {
17650 guard2 = true;
17651 }
17652 } else {
17653 guard2 = true;
17654 }
17655 }
17656
17657 if (guard2) {
17658 for (cartesian_trajectory_planner_B.minnanb = 0;
17659 cartesian_trajectory_planner_B.minnanb < 5;
17660 cartesian_trajectory_planner_B.minnanb++) {
17661 cartesian_trajectory_planner_B.c_vstr[cartesian_trajectory_planner_B.minnanb]
17662 = tmp_2[cartesian_trajectory_planner_B.minnanb];
17663 }
17664
17665 if (jtype->size[1] <= 5) {
17666 cartesian_trajectory_planner_B.loop_ub_g0 = jtype->size[1];
17667 for (cartesian_trajectory_planner_B.minnanb = 0;
17668 cartesian_trajectory_planner_B.minnanb < 5;
17669 cartesian_trajectory_planner_B.minnanb++) {
17670 cartesian_trajectory_planner_B.b_hm[cartesian_trajectory_planner_B.minnanb]
17671 = tmp_2[cartesian_trajectory_planner_B.minnanb];
17672 }
17673
17674 cartesian_trajectory_planner_B.b_bool_cg = false;
17675 cartesian_trajectory_planner_B.minnanb = jtype->size[1];
17676 if (cartesian_trajectory_planner_B.minnanb >= 5) {
17677 cartesian_trajectory_planner_B.minnanb = 5;
17678 }
17679
17680 guard11 = false;
17681 if (cartesian_trajectory_planner_B.loop_ub_g0 <=
17682 cartesian_trajectory_planner_B.minnanb) {
17683 if (cartesian_trajectory_planner_B.minnanb <
17684 cartesian_trajectory_planner_B.loop_ub_g0) {
17685 cartesian_trajectory_planner_B.loop_ub_g0 =
17686 cartesian_trajectory_planner_B.minnanb;
17687 }
17688
17689 cartesian_trajectory_planner_B.minnanb =
17690 cartesian_trajectory_planner_B.loop_ub_g0 - 1;
17691 guard11 = true;
17692 } else {
17693 if (jtype->size[1] == 5) {
17694 cartesian_trajectory_planner_B.minnanb = 4;
17695 guard11 = true;
17696 }
17697 }
17698
17699 if (guard11) {
17700 cartesian_trajectory_planner_B.loop_ub_g0 = 1;
17701 do {
17702 exitg1 = 0;
17703 if (cartesian_trajectory_planner_B.loop_ub_g0 - 1 <=
17704 cartesian_trajectory_planner_B.minnanb) {
17705 cartesian_trajectory_planner_B.kstr_c =
17706 cartesian_trajectory_planner_B.loop_ub_g0 - 1;
17707 if (tmp_3[static_cast<uint8_T>(jtype->
17708 data[cartesian_trajectory_planner_B.kstr_c]) & 127] != tmp_3[
17709 static_cast<int32_T>
17710 (cartesian_trajectory_planner_B.b_hm[cartesian_trajectory_planner_B.kstr_c])])
17711 {
17712 exitg1 = 1;
17713 } else {
17714 cartesian_trajectory_planner_B.loop_ub_g0++;
17715 }
17716 } else {
17717 cartesian_trajectory_planner_B.b_bool_cg = true;
17718 exitg1 = 1;
17719 }
17720 } while (exitg1 == 0);
17721 }
17722
17723 if (cartesian_trajectory_planner_B.b_bool_cg) {
17724 if (jtype->size[1] == 5) {
17725 cartesian_trajectory_planner_B.nmatched = 1;
17726 cartesian_trajectory_planner_B.partial_match_size_idx_1 = 5;
17727 for (cartesian_trajectory_planner_B.minnanb = 0;
17728 cartesian_trajectory_planner_B.minnanb < 5;
17729 cartesian_trajectory_planner_B.minnanb++) {
17730 cartesian_trajectory_planner_B.b_na[cartesian_trajectory_planner_B.minnanb]
17731 =
17732 cartesian_trajectory_planner_B.c_vstr[cartesian_trajectory_planner_B.minnanb];
17733 }
17734 } else {
17735 if (!cartesian_trajectory_planner_B.matched) {
17736 cartesian_trajectory_planner_B.partial_match_size_idx_1 = 5;
17737 for (cartesian_trajectory_planner_B.minnanb = 0;
17738 cartesian_trajectory_planner_B.minnanb < 5;
17739 cartesian_trajectory_planner_B.minnanb++) {
17740 cartesian_trajectory_planner_B.partial_match_data[cartesian_trajectory_planner_B.minnanb]
17741 =
17742 cartesian_trajectory_planner_B.c_vstr[cartesian_trajectory_planner_B.minnanb];
17743 }
17744 }
17745
17746 cartesian_trajectory_planner_B.nmatched++;
17747 guard1 = true;
17748 }
17749 } else {
17750 guard1 = true;
17751 }
17752 } else {
17753 guard1 = true;
17754 }
17755 }
17756
17757 if (guard1) {
17758 if (cartesian_trajectory_planner_B.nmatched == 0) {
17759 cartesian_trajectory_planner_B.partial_match_size_idx_1 = 0;
17760 } else {
17761 cartesian_trajectory_planner_B.loop_ub_g0 =
17762 cartesian_trajectory_planner_B.partial_match_size_idx_1 - 1;
17763 if (0 <= cartesian_trajectory_planner_B.loop_ub_g0) {
17764 memcpy(&cartesian_trajectory_planner_B.b_na[0],
17765 &cartesian_trajectory_planner_B.partial_match_data[0],
17766 (cartesian_trajectory_planner_B.loop_ub_g0 + 1) * sizeof(char_T));
17767 }
17768 }
17769 }
17770
17771 if ((cartesian_trajectory_planner_B.nmatched == 0) || ((jtype->size[1] == 0)
17772 != (cartesian_trajectory_planner_B.partial_match_size_idx_1 == 0))) {
17773 cartesian_trajectory_planner_B.partial_match_size_idx_1 = 0;
17774 } else {
17775 cartesian_trajectory_planner_B.loop_ub_g0 =
17776 cartesian_trajectory_planner_B.partial_match_size_idx_1 - 1;
17777 if (0 <= cartesian_trajectory_planner_B.loop_ub_g0) {
17778 memcpy(&cartesian_trajectory_planner_B.partial_match_data[0],
17779 &cartesian_trajectory_planner_B.b_na[0],
17780 (cartesian_trajectory_planner_B.loop_ub_g0 + 1) * sizeof(char_T));
17781 }
17782 }
17783
17784 cartesian_trajectory_planner_B.minnanb = obj->Type->size[0] * obj->Type->size
17785 [1];
17786 obj->Type->size[0] = 1;
17787 obj->Type->size[1] = cartesian_trajectory_planner_B.partial_match_size_idx_1;
17788 cartes_emxEnsureCapacity_char_T(obj->Type,
17789 cartesian_trajectory_planner_B.minnanb);
17790 cartesian_trajectory_planner_B.loop_ub_g0 =
17791 cartesian_trajectory_planner_B.partial_match_size_idx_1 - 1;
17792 for (cartesian_trajectory_planner_B.minnanb = 0;
17793 cartesian_trajectory_planner_B.minnanb <=
17794 cartesian_trajectory_planner_B.loop_ub_g0;
17795 cartesian_trajectory_planner_B.minnanb++) {
17796 obj->Type->data[cartesian_trajectory_planner_B.minnanb] =
17797 cartesian_trajectory_planner_B.partial_match_data[cartesian_trajectory_planner_B.minnanb];
17798 }
17799
17800 cartesian_trajec_emxInit_char_T(&switch_expression, 2);
17801 cartesian_trajectory_planner_B.minnanb = switch_expression->size[0] *
17802 switch_expression->size[1];
17803 switch_expression->size[0] = 1;
17804 switch_expression->size[1] = obj->Type->size[1];
17805 cartes_emxEnsureCapacity_char_T(switch_expression,
17806 cartesian_trajectory_planner_B.minnanb);
17807 cartesian_trajectory_planner_B.loop_ub_g0 = obj->Type->size[0] * obj->
17808 Type->size[1] - 1;
17809 for (cartesian_trajectory_planner_B.minnanb = 0;
17810 cartesian_trajectory_planner_B.minnanb <=
17811 cartesian_trajectory_planner_B.loop_ub_g0;
17812 cartesian_trajectory_planner_B.minnanb++) {
17813 switch_expression->data[cartesian_trajectory_planner_B.minnanb] = obj->
17814 Type->data[cartesian_trajectory_planner_B.minnanb];
17815 }
17816
17817 for (cartesian_trajectory_planner_B.minnanb = 0;
17818 cartesian_trajectory_planner_B.minnanb < 8;
17819 cartesian_trajectory_planner_B.minnanb++) {
17820 cartesian_trajectory_planner_B.b_cq[cartesian_trajectory_planner_B.minnanb] =
17821 tmp_0[cartesian_trajectory_planner_B.minnanb];
17822 }
17823
17824 cartesian_trajectory_planner_B.b_bool_cg = false;
17825 if (switch_expression->size[1] == 8) {
17826 cartesian_trajectory_planner_B.loop_ub_g0 = 1;
17827 do {
17828 exitg1 = 0;
17829 if (cartesian_trajectory_planner_B.loop_ub_g0 - 1 < 8) {
17830 cartesian_trajectory_planner_B.kstr_c =
17831 cartesian_trajectory_planner_B.loop_ub_g0 - 1;
17832 if (switch_expression->data[cartesian_trajectory_planner_B.kstr_c] !=
17833 cartesian_trajectory_planner_B.b_cq[cartesian_trajectory_planner_B.kstr_c])
17834 {
17835 exitg1 = 1;
17836 } else {
17837 cartesian_trajectory_planner_B.loop_ub_g0++;
17838 }
17839 } else {
17840 cartesian_trajectory_planner_B.b_bool_cg = true;
17841 exitg1 = 1;
17842 }
17843 } while (exitg1 == 0);
17844 }
17845
17846 if (cartesian_trajectory_planner_B.b_bool_cg) {
17847 cartesian_trajectory_planner_B.minnanb = 0;
17848 } else {
17849 for (cartesian_trajectory_planner_B.minnanb = 0;
17850 cartesian_trajectory_planner_B.minnanb < 9;
17851 cartesian_trajectory_planner_B.minnanb++) {
17852 cartesian_trajectory_planner_B.b_na[cartesian_trajectory_planner_B.minnanb]
17853 = tmp_1[cartesian_trajectory_planner_B.minnanb];
17854 }
17855
17856 cartesian_trajectory_planner_B.b_bool_cg = false;
17857 if (switch_expression->size[1] == 9) {
17858 cartesian_trajectory_planner_B.loop_ub_g0 = 1;
17859 do {
17860 exitg1 = 0;
17861 if (cartesian_trajectory_planner_B.loop_ub_g0 - 1 < 9) {
17862 cartesian_trajectory_planner_B.kstr_c =
17863 cartesian_trajectory_planner_B.loop_ub_g0 - 1;
17864 if (switch_expression->data[cartesian_trajectory_planner_B.kstr_c] !=
17865 cartesian_trajectory_planner_B.b_na[cartesian_trajectory_planner_B.kstr_c])
17866 {
17867 exitg1 = 1;
17868 } else {
17869 cartesian_trajectory_planner_B.loop_ub_g0++;
17870 }
17871 } else {
17872 cartesian_trajectory_planner_B.b_bool_cg = true;
17873 exitg1 = 1;
17874 }
17875 } while (exitg1 == 0);
17876 }
17877
17878 if (cartesian_trajectory_planner_B.b_bool_cg) {
17879 cartesian_trajectory_planner_B.minnanb = 1;
17880 } else {
17881 cartesian_trajectory_planner_B.minnanb = -1;
17882 }
17883 }
17884
17885 cartesian_trajec_emxFree_char_T(&switch_expression);
17886 switch (cartesian_trajectory_planner_B.minnanb) {
17887 case 0:
17888 cartesian_trajectory_planner_B.iv3[0] = 0;
17889 cartesian_trajectory_planner_B.iv3[1] = 0;
17890 cartesian_trajectory_planner_B.iv3[2] = 1;
17891 cartesian_trajectory_planner_B.iv3[3] = 0;
17892 cartesian_trajectory_planner_B.iv3[4] = 0;
17893 cartesian_trajectory_planner_B.iv3[5] = 0;
17894 for (cartesian_trajectory_planner_B.minnanb = 0;
17895 cartesian_trajectory_planner_B.minnanb < 6;
17896 cartesian_trajectory_planner_B.minnanb++) {
17897 cartesian_trajectory_planner_B.msubspace_data_m[cartesian_trajectory_planner_B.minnanb]
17898 =
17899 cartesian_trajectory_planner_B.iv3[cartesian_trajectory_planner_B.minnanb];
17900 }
17901
17902 cartesian_trajectory_planner_B.poslim_data_i[0] = -3.1415926535897931;
17903 cartesian_trajectory_planner_B.poslim_data_i[1] = 3.1415926535897931;
17904 obj->VelocityNumber = 1.0;
17905 obj->PositionNumber = 1.0;
17906 obj->JointAxisInternal[0] = 0.0;
17907 obj->JointAxisInternal[1] = 0.0;
17908 obj->JointAxisInternal[2] = 1.0;
17909 break;
17910
17911 case 1:
17912 cartesian_trajectory_planner_B.iv3[0] = 0;
17913 cartesian_trajectory_planner_B.iv3[1] = 0;
17914 cartesian_trajectory_planner_B.iv3[2] = 0;
17915 cartesian_trajectory_planner_B.iv3[3] = 0;
17916 cartesian_trajectory_planner_B.iv3[4] = 0;
17917 cartesian_trajectory_planner_B.iv3[5] = 1;
17918 for (cartesian_trajectory_planner_B.minnanb = 0;
17919 cartesian_trajectory_planner_B.minnanb < 6;
17920 cartesian_trajectory_planner_B.minnanb++) {
17921 cartesian_trajectory_planner_B.msubspace_data_m[cartesian_trajectory_planner_B.minnanb]
17922 =
17923 cartesian_trajectory_planner_B.iv3[cartesian_trajectory_planner_B.minnanb];
17924 }
17925
17926 cartesian_trajectory_planner_B.poslim_data_i[0] = -0.5;
17927 cartesian_trajectory_planner_B.poslim_data_i[1] = 0.5;
17928 obj->VelocityNumber = 1.0;
17929 obj->PositionNumber = 1.0;
17930 obj->JointAxisInternal[0] = 0.0;
17931 obj->JointAxisInternal[1] = 0.0;
17932 obj->JointAxisInternal[2] = 1.0;
17933 break;
17934
17935 default:
17936 for (cartesian_trajectory_planner_B.minnanb = 0;
17937 cartesian_trajectory_planner_B.minnanb < 6;
17938 cartesian_trajectory_planner_B.minnanb++) {
17939 cartesian_trajectory_planner_B.msubspace_data_m[cartesian_trajectory_planner_B.minnanb]
17940 = 0;
17941 }
17942
17943 cartesian_trajectory_planner_B.poslim_data_i[0] = 0.0;
17944 cartesian_trajectory_planner_B.poslim_data_i[1] = 0.0;
17945 obj->VelocityNumber = 0.0;
17946 obj->PositionNumber = 0.0;
17947 obj->JointAxisInternal[0] = 0.0;
17948 obj->JointAxisInternal[1] = 0.0;
17949 obj->JointAxisInternal[2] = 0.0;
17950 break;
17951 }
17952
17953 cartesian_trajectory_planner_B.minnanb = obj->MotionSubspace->size[0] *
17954 obj->MotionSubspace->size[1];
17955 obj->MotionSubspace->size[0] = 6;
17956 obj->MotionSubspace->size[1] = 1;
17957 cartes_emxEnsureCapacity_real_T(obj->MotionSubspace,
17958 cartesian_trajectory_planner_B.minnanb);
17959 for (cartesian_trajectory_planner_B.minnanb = 0;
17960 cartesian_trajectory_planner_B.minnanb < 6;
17961 cartesian_trajectory_planner_B.minnanb++) {
17962 obj->MotionSubspace->data[cartesian_trajectory_planner_B.minnanb] =
17963 cartesian_trajectory_planner_B.msubspace_data_m[cartesian_trajectory_planner_B.minnanb];
17964 }
17965
17966 cartesian_trajectory_planner_B.minnanb = obj->PositionLimitsInternal->size[0] *
17967 obj->PositionLimitsInternal->size[1];
17968 obj->PositionLimitsInternal->size[0] = 1;
17969 obj->PositionLimitsInternal->size[1] = 2;
17970 cartes_emxEnsureCapacity_real_T(obj->PositionLimitsInternal,
17971 cartesian_trajectory_planner_B.minnanb);
17972 for (cartesian_trajectory_planner_B.minnanb = 0;
17973 cartesian_trajectory_planner_B.minnanb < 2;
17974 cartesian_trajectory_planner_B.minnanb++) {
17975 obj->PositionLimitsInternal->data[cartesian_trajectory_planner_B.minnanb] =
17976 cartesian_trajectory_planner_B.poslim_data_i[cartesian_trajectory_planner_B.minnanb];
17977 }
17978
17979 cartesian_trajectory_planner_B.minnanb = obj->HomePositionInternal->size[0];
17980 obj->HomePositionInternal->size[0] = 1;
17981 cartes_emxEnsureCapacity_real_T(obj->HomePositionInternal,
17982 cartesian_trajectory_planner_B.minnanb);
17983 for (cartesian_trajectory_planner_B.minnanb = 0;
17984 cartesian_trajectory_planner_B.minnanb < 1;
17985 cartesian_trajectory_planner_B.minnanb++) {
17986 obj->HomePositionInternal->data[0] = 0.0;
17987 }
17988
17989 return b_obj;
17990}
17991
17992static w_robotics_manip_internal_Rig_T *cartesian_trajec_RigidBody_copy(const
17993 v_robotics_manip_internal_Rig_T *obj, c_rigidBodyJoint_cartesian__a_T *iobj_0,
17994 c_rigidBodyJoint_cartesian__a_T *iobj_1, w_robotics_manip_internal_Rig_T
17995 *iobj_2)
17996{
17997 w_robotics_manip_internal_Rig_T *newbody;
17998 c_rigidBodyJoint_cartesian__a_T *newjoint;
17999 emxArray_char_T_cartesian_tra_T *jtype;
18000 emxArray_char_T_cartesian_tra_T *jname;
18001 emxArray_real_T_cartesian_tra_T *obj_0;
18002 emxArray_real_T_cartesian_tra_T *obj_1;
18003 emxArray_real_T_cartesian_tra_T *obj_2;
18004 static const int8_T tmp[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1
18005 };
18006
18007 static const char_T tmp_0[5] = { 'f', 'i', 'x', 'e', 'd' };
18008
18009 static const char_T tmp_1[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
18010
18011 static const char_T tmp_2[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
18012
18013 int32_T exitg1;
18014 cartesian_trajec_emxInit_char_T(&jtype, 2);
18015 cartesian_trajectory_planner_B.b_kstr_ld = jtype->size[0] * jtype->size[1];
18016 jtype->size[0] = 1;
18017 jtype->size[1] = obj->NameInternal->size[1];
18018 cartes_emxEnsureCapacity_char_T(jtype,
18019 cartesian_trajectory_planner_B.b_kstr_ld);
18020 cartesian_trajectory_planner_B.loop_ub_hb = obj->NameInternal->size[0] *
18021 obj->NameInternal->size[1] - 1;
18022 for (cartesian_trajectory_planner_B.b_kstr_ld = 0;
18023 cartesian_trajectory_planner_B.b_kstr_ld <=
18024 cartesian_trajectory_planner_B.loop_ub_hb;
18025 cartesian_trajectory_planner_B.b_kstr_ld++) {
18026 jtype->data[cartesian_trajectory_planner_B.b_kstr_ld] = obj->
18027 NameInternal->data[cartesian_trajectory_planner_B.b_kstr_ld];
18028 }
18029
18030 newbody = iobj_2;
18031 cartesian_trajectory_planner_B.b_kstr_ld = iobj_2->NameInternal->size[0] *
18032 iobj_2->NameInternal->size[1];
18033 iobj_2->NameInternal->size[0] = 1;
18034 iobj_2->NameInternal->size[1] = jtype->size[1];
18035 cartes_emxEnsureCapacity_char_T(iobj_2->NameInternal,
18036 cartesian_trajectory_planner_B.b_kstr_ld);
18037 cartesian_trajectory_planner_B.loop_ub_hb = jtype->size[0] * jtype->size[1] -
18038 1;
18039 for (cartesian_trajectory_planner_B.b_kstr_ld = 0;
18040 cartesian_trajectory_planner_B.b_kstr_ld <=
18041 cartesian_trajectory_planner_B.loop_ub_hb;
18042 cartesian_trajectory_planner_B.b_kstr_ld++) {
18043 iobj_2->NameInternal->data[cartesian_trajectory_planner_B.b_kstr_ld] =
18044 jtype->data[cartesian_trajectory_planner_B.b_kstr_ld];
18045 }
18046
18047 iobj_0->InTree = false;
18048 for (cartesian_trajectory_planner_B.b_kstr_ld = 0;
18049 cartesian_trajectory_planner_B.b_kstr_ld < 16;
18050 cartesian_trajectory_planner_B.b_kstr_ld++) {
18051 iobj_0->JointToParentTransform[cartesian_trajectory_planner_B.b_kstr_ld] =
18052 tmp[cartesian_trajectory_planner_B.b_kstr_ld];
18053 }
18054
18055 for (cartesian_trajectory_planner_B.b_kstr_ld = 0;
18056 cartesian_trajectory_planner_B.b_kstr_ld < 16;
18057 cartesian_trajectory_planner_B.b_kstr_ld++) {
18058 iobj_0->ChildToJointTransform[cartesian_trajectory_planner_B.b_kstr_ld] =
18059 tmp[cartesian_trajectory_planner_B.b_kstr_ld];
18060 }
18061
18062 cartesian_trajectory_planner_B.b_kstr_ld = iobj_0->NameInternal->size[0] *
18063 iobj_0->NameInternal->size[1];
18064 iobj_0->NameInternal->size[0] = 1;
18065 iobj_0->NameInternal->size[1] = jtype->size[1] + 4;
18066 cartes_emxEnsureCapacity_char_T(iobj_0->NameInternal,
18067 cartesian_trajectory_planner_B.b_kstr_ld);
18068 cartesian_trajectory_planner_B.loop_ub_hb = jtype->size[0] * jtype->size[1];
18069 for (cartesian_trajectory_planner_B.b_kstr_ld = 0;
18070 cartesian_trajectory_planner_B.b_kstr_ld <
18071 cartesian_trajectory_planner_B.loop_ub_hb;
18072 cartesian_trajectory_planner_B.b_kstr_ld++) {
18073 iobj_0->NameInternal->data[cartesian_trajectory_planner_B.b_kstr_ld] =
18074 jtype->data[cartesian_trajectory_planner_B.b_kstr_ld];
18075 }
18076
18077 iobj_0->NameInternal->data[cartesian_trajectory_planner_B.loop_ub_hb] = '_';
18078 iobj_0->NameInternal->data[cartesian_trajectory_planner_B.loop_ub_hb + 1] =
18079 'j';
18080 iobj_0->NameInternal->data[cartesian_trajectory_planner_B.loop_ub_hb + 2] =
18081 'n';
18082 iobj_0->NameInternal->data[cartesian_trajectory_planner_B.loop_ub_hb + 3] =
18083 't';
18084 cartesian_trajectory_planner_B.b_kstr_ld = iobj_0->Type->size[0] *
18085 iobj_0->Type->size[1];
18086 iobj_0->Type->size[0] = 1;
18087 iobj_0->Type->size[1] = 5;
18088 cartes_emxEnsureCapacity_char_T(iobj_0->Type,
18089 cartesian_trajectory_planner_B.b_kstr_ld);
18090 for (cartesian_trajectory_planner_B.b_kstr_ld = 0;
18091 cartesian_trajectory_planner_B.b_kstr_ld < 5;
18092 cartesian_trajectory_planner_B.b_kstr_ld++) {
18093 iobj_0->Type->data[cartesian_trajectory_planner_B.b_kstr_ld] =
18094 tmp_0[cartesian_trajectory_planner_B.b_kstr_ld];
18095 }
18096
18097 cartesian_trajectory_planner_B.b_kstr_ld = jtype->size[0] * jtype->size[1];
18098 jtype->size[0] = 1;
18099 jtype->size[1] = iobj_0->Type->size[1];
18100 cartes_emxEnsureCapacity_char_T(jtype,
18101 cartesian_trajectory_planner_B.b_kstr_ld);
18102 cartesian_trajectory_planner_B.loop_ub_hb = iobj_0->Type->size[0] *
18103 iobj_0->Type->size[1] - 1;
18104 for (cartesian_trajectory_planner_B.b_kstr_ld = 0;
18105 cartesian_trajectory_planner_B.b_kstr_ld <=
18106 cartesian_trajectory_planner_B.loop_ub_hb;
18107 cartesian_trajectory_planner_B.b_kstr_ld++) {
18108 jtype->data[cartesian_trajectory_planner_B.b_kstr_ld] = iobj_0->Type->
18109 data[cartesian_trajectory_planner_B.b_kstr_ld];
18110 }
18111
18112 for (cartesian_trajectory_planner_B.b_kstr_ld = 0;
18113 cartesian_trajectory_planner_B.b_kstr_ld < 8;
18114 cartesian_trajectory_planner_B.b_kstr_ld++) {
18115 cartesian_trajectory_planner_B.b_gg[cartesian_trajectory_planner_B.b_kstr_ld]
18116 = tmp_1[cartesian_trajectory_planner_B.b_kstr_ld];
18117 }
18118
18119 cartesian_trajectory_planner_B.b_bool_k = false;
18120 if (jtype->size[1] == 8) {
18121 cartesian_trajectory_planner_B.b_kstr_ld = 1;
18122 do {
18123 exitg1 = 0;
18124 if (cartesian_trajectory_planner_B.b_kstr_ld - 1 < 8) {
18125 cartesian_trajectory_planner_B.loop_ub_hb =
18126 cartesian_trajectory_planner_B.b_kstr_ld - 1;
18127 if (jtype->data[cartesian_trajectory_planner_B.loop_ub_hb] !=
18128 cartesian_trajectory_planner_B.b_gg[cartesian_trajectory_planner_B.loop_ub_hb])
18129 {
18130 exitg1 = 1;
18131 } else {
18132 cartesian_trajectory_planner_B.b_kstr_ld++;
18133 }
18134 } else {
18135 cartesian_trajectory_planner_B.b_bool_k = true;
18136 exitg1 = 1;
18137 }
18138 } while (exitg1 == 0);
18139 }
18140
18141 if (cartesian_trajectory_planner_B.b_bool_k) {
18142 cartesian_trajectory_planner_B.b_kstr_ld = 0;
18143 } else {
18144 for (cartesian_trajectory_planner_B.b_kstr_ld = 0;
18145 cartesian_trajectory_planner_B.b_kstr_ld < 9;
18146 cartesian_trajectory_planner_B.b_kstr_ld++) {
18147 cartesian_trajectory_planner_B.b_d[cartesian_trajectory_planner_B.b_kstr_ld]
18148 = tmp_2[cartesian_trajectory_planner_B.b_kstr_ld];
18149 }
18150
18151 cartesian_trajectory_planner_B.b_bool_k = false;
18152 if (jtype->size[1] == 9) {
18153 cartesian_trajectory_planner_B.b_kstr_ld = 1;
18154 do {
18155 exitg1 = 0;
18156 if (cartesian_trajectory_planner_B.b_kstr_ld - 1 < 9) {
18157 cartesian_trajectory_planner_B.loop_ub_hb =
18158 cartesian_trajectory_planner_B.b_kstr_ld - 1;
18159 if (jtype->data[cartesian_trajectory_planner_B.loop_ub_hb] !=
18160 cartesian_trajectory_planner_B.b_d[cartesian_trajectory_planner_B.loop_ub_hb])
18161 {
18162 exitg1 = 1;
18163 } else {
18164 cartesian_trajectory_planner_B.b_kstr_ld++;
18165 }
18166 } else {
18167 cartesian_trajectory_planner_B.b_bool_k = true;
18168 exitg1 = 1;
18169 }
18170 } while (exitg1 == 0);
18171 }
18172
18173 if (cartesian_trajectory_planner_B.b_bool_k) {
18174 cartesian_trajectory_planner_B.b_kstr_ld = 1;
18175 } else {
18176 cartesian_trajectory_planner_B.b_kstr_ld = -1;
18177 }
18178 }
18179
18180 switch (cartesian_trajectory_planner_B.b_kstr_ld) {
18181 case 0:
18182 cartesian_trajectory_planner_B.iv2[0] = 0;
18183 cartesian_trajectory_planner_B.iv2[1] = 0;
18184 cartesian_trajectory_planner_B.iv2[2] = 1;
18185 cartesian_trajectory_planner_B.iv2[3] = 0;
18186 cartesian_trajectory_planner_B.iv2[4] = 0;
18187 cartesian_trajectory_planner_B.iv2[5] = 0;
18188 for (cartesian_trajectory_planner_B.b_kstr_ld = 0;
18189 cartesian_trajectory_planner_B.b_kstr_ld < 6;
18190 cartesian_trajectory_planner_B.b_kstr_ld++) {
18191 cartesian_trajectory_planner_B.msubspace_data_o[cartesian_trajectory_planner_B.b_kstr_ld]
18192 =
18193 cartesian_trajectory_planner_B.iv2[cartesian_trajectory_planner_B.b_kstr_ld];
18194 }
18195
18196 cartesian_trajectory_planner_B.poslim_data_as[0] = -3.1415926535897931;
18197 cartesian_trajectory_planner_B.poslim_data_as[1] = 3.1415926535897931;
18198 iobj_0->VelocityNumber = 1.0;
18199 iobj_0->PositionNumber = 1.0;
18200 iobj_0->JointAxisInternal[0] = 0.0;
18201 iobj_0->JointAxisInternal[1] = 0.0;
18202 iobj_0->JointAxisInternal[2] = 1.0;
18203 break;
18204
18205 case 1:
18206 cartesian_trajectory_planner_B.iv2[0] = 0;
18207 cartesian_trajectory_planner_B.iv2[1] = 0;
18208 cartesian_trajectory_planner_B.iv2[2] = 0;
18209 cartesian_trajectory_planner_B.iv2[3] = 0;
18210 cartesian_trajectory_planner_B.iv2[4] = 0;
18211 cartesian_trajectory_planner_B.iv2[5] = 1;
18212 for (cartesian_trajectory_planner_B.b_kstr_ld = 0;
18213 cartesian_trajectory_planner_B.b_kstr_ld < 6;
18214 cartesian_trajectory_planner_B.b_kstr_ld++) {
18215 cartesian_trajectory_planner_B.msubspace_data_o[cartesian_trajectory_planner_B.b_kstr_ld]
18216 =
18217 cartesian_trajectory_planner_B.iv2[cartesian_trajectory_planner_B.b_kstr_ld];
18218 }
18219
18220 cartesian_trajectory_planner_B.poslim_data_as[0] = -0.5;
18221 cartesian_trajectory_planner_B.poslim_data_as[1] = 0.5;
18222 iobj_0->VelocityNumber = 1.0;
18223 iobj_0->PositionNumber = 1.0;
18224 iobj_0->JointAxisInternal[0] = 0.0;
18225 iobj_0->JointAxisInternal[1] = 0.0;
18226 iobj_0->JointAxisInternal[2] = 1.0;
18227 break;
18228
18229 default:
18230 for (cartesian_trajectory_planner_B.b_kstr_ld = 0;
18231 cartesian_trajectory_planner_B.b_kstr_ld < 6;
18232 cartesian_trajectory_planner_B.b_kstr_ld++) {
18233 cartesian_trajectory_planner_B.msubspace_data_o[cartesian_trajectory_planner_B.b_kstr_ld]
18234 = 0;
18235 }
18236
18237 cartesian_trajectory_planner_B.poslim_data_as[0] = 0.0;
18238 cartesian_trajectory_planner_B.poslim_data_as[1] = 0.0;
18239 iobj_0->VelocityNumber = 0.0;
18240 iobj_0->PositionNumber = 0.0;
18241 iobj_0->JointAxisInternal[0] = 0.0;
18242 iobj_0->JointAxisInternal[1] = 0.0;
18243 iobj_0->JointAxisInternal[2] = 0.0;
18244 break;
18245 }
18246
18247 cartesian_trajectory_planner_B.b_kstr_ld = iobj_0->MotionSubspace->size[0] *
18248 iobj_0->MotionSubspace->size[1];
18249 iobj_0->MotionSubspace->size[0] = 6;
18250 iobj_0->MotionSubspace->size[1] = 1;
18251 cartes_emxEnsureCapacity_real_T(iobj_0->MotionSubspace,
18252 cartesian_trajectory_planner_B.b_kstr_ld);
18253 for (cartesian_trajectory_planner_B.b_kstr_ld = 0;
18254 cartesian_trajectory_planner_B.b_kstr_ld < 6;
18255 cartesian_trajectory_planner_B.b_kstr_ld++) {
18256 iobj_0->MotionSubspace->data[cartesian_trajectory_planner_B.b_kstr_ld] =
18257 cartesian_trajectory_planner_B.msubspace_data_o[cartesian_trajectory_planner_B.b_kstr_ld];
18258 }
18259
18260 cartesian_trajectory_planner_B.b_kstr_ld = iobj_0->
18261 PositionLimitsInternal->size[0] * iobj_0->PositionLimitsInternal->size[1];
18262 iobj_0->PositionLimitsInternal->size[0] = 1;
18263 iobj_0->PositionLimitsInternal->size[1] = 2;
18264 cartes_emxEnsureCapacity_real_T(iobj_0->PositionLimitsInternal,
18265 cartesian_trajectory_planner_B.b_kstr_ld);
18266 for (cartesian_trajectory_planner_B.b_kstr_ld = 0;
18267 cartesian_trajectory_planner_B.b_kstr_ld < 2;
18268 cartesian_trajectory_planner_B.b_kstr_ld++) {
18269 iobj_0->PositionLimitsInternal->
18270 data[cartesian_trajectory_planner_B.b_kstr_ld] =
18271 cartesian_trajectory_planner_B.poslim_data_as[cartesian_trajectory_planner_B.b_kstr_ld];
18272 }
18273
18274 cartesian_trajectory_planner_B.b_kstr_ld = iobj_0->HomePositionInternal->size
18275 [0];
18276 iobj_0->HomePositionInternal->size[0] = 1;
18277 cartes_emxEnsureCapacity_real_T(iobj_0->HomePositionInternal,
18278 cartesian_trajectory_planner_B.b_kstr_ld);
18279 for (cartesian_trajectory_planner_B.b_kstr_ld = 0;
18280 cartesian_trajectory_planner_B.b_kstr_ld < 1;
18281 cartesian_trajectory_planner_B.b_kstr_ld++) {
18282 iobj_0->HomePositionInternal->data[0] = 0.0;
18283 }
18284
18285 iobj_2->JointInternal = iobj_0;
18286 iobj_2->Index = -1.0;
18287 iobj_2->ParentIndex = -1.0;
18288 cartesian_trajectory_planner_B.b_kstr_ld = jtype->size[0] * jtype->size[1];
18289 jtype->size[0] = 1;
18290 jtype->size[1] = obj->JointInternal.Type->size[1];
18291 cartes_emxEnsureCapacity_char_T(jtype,
18292 cartesian_trajectory_planner_B.b_kstr_ld);
18293 cartesian_trajectory_planner_B.loop_ub_hb = obj->JointInternal.Type->size[0] *
18294 obj->JointInternal.Type->size[1] - 1;
18295 for (cartesian_trajectory_planner_B.b_kstr_ld = 0;
18296 cartesian_trajectory_planner_B.b_kstr_ld <=
18297 cartesian_trajectory_planner_B.loop_ub_hb;
18298 cartesian_trajectory_planner_B.b_kstr_ld++) {
18299 jtype->data[cartesian_trajectory_planner_B.b_kstr_ld] =
18300 obj->JointInternal.Type->data[cartesian_trajectory_planner_B.b_kstr_ld];
18301 }
18302
18303 cartesian_trajec_emxInit_char_T(&jname, 2);
18304 cartesian_trajectory_planner_B.b_kstr_ld = jname->size[0] * jname->size[1];
18305 jname->size[0] = 1;
18306 jname->size[1] = obj->JointInternal.NameInternal->size[1];
18307 cartes_emxEnsureCapacity_char_T(jname,
18308 cartesian_trajectory_planner_B.b_kstr_ld);
18309 cartesian_trajectory_planner_B.loop_ub_hb = obj->
18310 JointInternal.NameInternal->size[0] * obj->JointInternal.NameInternal->size
18311 [1] - 1;
18312 for (cartesian_trajectory_planner_B.b_kstr_ld = 0;
18313 cartesian_trajectory_planner_B.b_kstr_ld <=
18314 cartesian_trajectory_planner_B.loop_ub_hb;
18315 cartesian_trajectory_planner_B.b_kstr_ld++) {
18316 jname->data[cartesian_trajectory_planner_B.b_kstr_ld] =
18317 obj->JointInternal.NameInternal->
18318 data[cartesian_trajectory_planner_B.b_kstr_ld];
18319 }
18320
18321 newjoint = c_rigidBodyJoint_rigidBodyJoint(iobj_1, jname, jtype);
18322 cartesian_trajectory_planner_B.b_kstr_ld = jtype->size[0] * jtype->size[1];
18323 jtype->size[0] = 1;
18324 jtype->size[1] = obj->JointInternal.NameInternal->size[1];
18325 cartes_emxEnsureCapacity_char_T(jtype,
18326 cartesian_trajectory_planner_B.b_kstr_ld);
18327 cartesian_trajectory_planner_B.loop_ub_hb = obj->
18328 JointInternal.NameInternal->size[0] * obj->JointInternal.NameInternal->size
18329 [1] - 1;
18330 cartesian_trajec_emxFree_char_T(&jname);
18331 for (cartesian_trajectory_planner_B.b_kstr_ld = 0;
18332 cartesian_trajectory_planner_B.b_kstr_ld <=
18333 cartesian_trajectory_planner_B.loop_ub_hb;
18334 cartesian_trajectory_planner_B.b_kstr_ld++) {
18335 jtype->data[cartesian_trajectory_planner_B.b_kstr_ld] =
18336 obj->JointInternal.NameInternal->
18337 data[cartesian_trajectory_planner_B.b_kstr_ld];
18338 }
18339
18340 if (jtype->size[1] != 0) {
18341 cartesian_trajectory_planner_B.b_kstr_ld = jtype->size[0] * jtype->size[1];
18342 jtype->size[0] = 1;
18343 jtype->size[1] = obj->JointInternal.NameInternal->size[1];
18344 cartes_emxEnsureCapacity_char_T(jtype,
18345 cartesian_trajectory_planner_B.b_kstr_ld);
18346 cartesian_trajectory_planner_B.loop_ub_hb = obj->
18347 JointInternal.NameInternal->size[0] * obj->
18348 JointInternal.NameInternal->size[1] - 1;
18349 for (cartesian_trajectory_planner_B.b_kstr_ld = 0;
18350 cartesian_trajectory_planner_B.b_kstr_ld <=
18351 cartesian_trajectory_planner_B.loop_ub_hb;
18352 cartesian_trajectory_planner_B.b_kstr_ld++) {
18353 jtype->data[cartesian_trajectory_planner_B.b_kstr_ld] =
18354 obj->JointInternal.NameInternal->
18355 data[cartesian_trajectory_planner_B.b_kstr_ld];
18356 }
18357
18358 if (!newjoint->InTree) {
18359 cartesian_trajectory_planner_B.b_kstr_ld = newjoint->NameInternal->size[0]
18360 * newjoint->NameInternal->size[1];
18361 newjoint->NameInternal->size[0] = 1;
18362 newjoint->NameInternal->size[1] = jtype->size[1];
18363 cartes_emxEnsureCapacity_char_T(newjoint->NameInternal,
18364 cartesian_trajectory_planner_B.b_kstr_ld);
18365 cartesian_trajectory_planner_B.loop_ub_hb = jtype->size[0] * jtype->size[1]
18366 - 1;
18367 for (cartesian_trajectory_planner_B.b_kstr_ld = 0;
18368 cartesian_trajectory_planner_B.b_kstr_ld <=
18369 cartesian_trajectory_planner_B.loop_ub_hb;
18370 cartesian_trajectory_planner_B.b_kstr_ld++) {
18371 newjoint->NameInternal->data[cartesian_trajectory_planner_B.b_kstr_ld] =
18372 jtype->data[cartesian_trajectory_planner_B.b_kstr_ld];
18373 }
18374 }
18375 }
18376
18377 cartesian_trajec_emxFree_char_T(&jtype);
18378 cartesian_trajec_emxInit_real_T(&obj_0, 1);
18379 cartesian_trajectory_planner_B.loop_ub_hb =
18380 obj->JointInternal.PositionLimitsInternal->size[0] *
18381 obj->JointInternal.PositionLimitsInternal->size[1];
18382 cartesian_trajectory_planner_B.b_kstr_ld = newjoint->
18383 PositionLimitsInternal->size[0] * newjoint->PositionLimitsInternal->size[1];
18384 newjoint->PositionLimitsInternal->size[0] =
18385 obj->JointInternal.PositionLimitsInternal->size[0];
18386 newjoint->PositionLimitsInternal->size[1] = 2;
18387 cartes_emxEnsureCapacity_real_T(newjoint->PositionLimitsInternal,
18388 cartesian_trajectory_planner_B.b_kstr_ld);
18389 cartesian_trajectory_planner_B.b_kstr_ld = obj_0->size[0];
18390 obj_0->size[0] = cartesian_trajectory_planner_B.loop_ub_hb;
18391 cartes_emxEnsureCapacity_real_T(obj_0,
18392 cartesian_trajectory_planner_B.b_kstr_ld);
18393 for (cartesian_trajectory_planner_B.b_kstr_ld = 0;
18394 cartesian_trajectory_planner_B.b_kstr_ld <
18395 cartesian_trajectory_planner_B.loop_ub_hb;
18396 cartesian_trajectory_planner_B.b_kstr_ld++) {
18397 obj_0->data[cartesian_trajectory_planner_B.b_kstr_ld] =
18398 obj->JointInternal.PositionLimitsInternal->
18399 data[cartesian_trajectory_planner_B.b_kstr_ld];
18400 }
18401
18402 cartesian_trajectory_planner_B.loop_ub_hb = obj_0->size[0];
18403 for (cartesian_trajectory_planner_B.b_kstr_ld = 0;
18404 cartesian_trajectory_planner_B.b_kstr_ld <
18405 cartesian_trajectory_planner_B.loop_ub_hb;
18406 cartesian_trajectory_planner_B.b_kstr_ld++) {
18407 newjoint->PositionLimitsInternal->
18408 data[cartesian_trajectory_planner_B.b_kstr_ld] = obj_0->
18409 data[cartesian_trajectory_planner_B.b_kstr_ld];
18410 }
18411
18412 cartesian_trajec_emxFree_real_T(&obj_0);
18413 cartesian_trajec_emxInit_real_T(&obj_1, 1);
18414 cartesian_trajectory_planner_B.b_kstr_ld = obj_1->size[0];
18415 obj_1->size[0] = obj->JointInternal.HomePositionInternal->size[0];
18416 cartes_emxEnsureCapacity_real_T(obj_1,
18417 cartesian_trajectory_planner_B.b_kstr_ld);
18418 cartesian_trajectory_planner_B.loop_ub_hb =
18419 obj->JointInternal.HomePositionInternal->size[0];
18420 for (cartesian_trajectory_planner_B.b_kstr_ld = 0;
18421 cartesian_trajectory_planner_B.b_kstr_ld <
18422 cartesian_trajectory_planner_B.loop_ub_hb;
18423 cartesian_trajectory_planner_B.b_kstr_ld++) {
18424 obj_1->data[cartesian_trajectory_planner_B.b_kstr_ld] =
18425 obj->JointInternal.HomePositionInternal->
18426 data[cartesian_trajectory_planner_B.b_kstr_ld];
18427 }
18428
18429 cartesian_trajectory_planner_B.b_kstr_ld = newjoint->
18430 HomePositionInternal->size[0];
18431 newjoint->HomePositionInternal->size[0] = obj_1->size[0];
18432 cartes_emxEnsureCapacity_real_T(newjoint->HomePositionInternal,
18433 cartesian_trajectory_planner_B.b_kstr_ld);
18434 cartesian_trajectory_planner_B.loop_ub_hb = obj_1->size[0];
18435 for (cartesian_trajectory_planner_B.b_kstr_ld = 0;
18436 cartesian_trajectory_planner_B.b_kstr_ld <
18437 cartesian_trajectory_planner_B.loop_ub_hb;
18438 cartesian_trajectory_planner_B.b_kstr_ld++) {
18439 newjoint->HomePositionInternal->
18440 data[cartesian_trajectory_planner_B.b_kstr_ld] = obj_1->
18441 data[cartesian_trajectory_planner_B.b_kstr_ld];
18442 }
18443
18444 cartesian_trajec_emxFree_real_T(&obj_1);
18445 cartesian_trajectory_planner_B.obj_idx_0 =
18446 obj->JointInternal.JointAxisInternal[0];
18447 cartesian_trajectory_planner_B.obj_idx_1 =
18448 obj->JointInternal.JointAxisInternal[1];
18449 cartesian_trajectory_planner_B.obj_idx_2 =
18450 obj->JointInternal.JointAxisInternal[2];
18451 newjoint->JointAxisInternal[0] = cartesian_trajectory_planner_B.obj_idx_0;
18452 newjoint->JointAxisInternal[1] = cartesian_trajectory_planner_B.obj_idx_1;
18453 newjoint->JointAxisInternal[2] = cartesian_trajectory_planner_B.obj_idx_2;
18454 cartesian_trajec_emxInit_real_T(&obj_2, 1);
18455 cartesian_trajectory_planner_B.loop_ub_hb = obj->
18456 JointInternal.MotionSubspace->size[0] * obj->
18457 JointInternal.MotionSubspace->size[1];
18458 cartesian_trajectory_planner_B.b_kstr_ld = newjoint->MotionSubspace->size[0] *
18459 newjoint->MotionSubspace->size[1];
18460 newjoint->MotionSubspace->size[0] = 6;
18461 newjoint->MotionSubspace->size[1] = obj->JointInternal.MotionSubspace->size[1];
18462 cartes_emxEnsureCapacity_real_T(newjoint->MotionSubspace,
18463 cartesian_trajectory_planner_B.b_kstr_ld);
18464 cartesian_trajectory_planner_B.b_kstr_ld = obj_2->size[0];
18465 obj_2->size[0] = cartesian_trajectory_planner_B.loop_ub_hb;
18466 cartes_emxEnsureCapacity_real_T(obj_2,
18467 cartesian_trajectory_planner_B.b_kstr_ld);
18468 for (cartesian_trajectory_planner_B.b_kstr_ld = 0;
18469 cartesian_trajectory_planner_B.b_kstr_ld <
18470 cartesian_trajectory_planner_B.loop_ub_hb;
18471 cartesian_trajectory_planner_B.b_kstr_ld++) {
18472 obj_2->data[cartesian_trajectory_planner_B.b_kstr_ld] =
18473 obj->JointInternal.MotionSubspace->
18474 data[cartesian_trajectory_planner_B.b_kstr_ld];
18475 }
18476
18477 cartesian_trajectory_planner_B.loop_ub_hb = obj_2->size[0];
18478 for (cartesian_trajectory_planner_B.b_kstr_ld = 0;
18479 cartesian_trajectory_planner_B.b_kstr_ld <
18480 cartesian_trajectory_planner_B.loop_ub_hb;
18481 cartesian_trajectory_planner_B.b_kstr_ld++) {
18482 newjoint->MotionSubspace->data[cartesian_trajectory_planner_B.b_kstr_ld] =
18483 obj_2->data[cartesian_trajectory_planner_B.b_kstr_ld];
18484 }
18485
18486 cartesian_trajec_emxFree_real_T(&obj_2);
18487 for (cartesian_trajectory_planner_B.b_kstr_ld = 0;
18488 cartesian_trajectory_planner_B.b_kstr_ld < 16;
18489 cartesian_trajectory_planner_B.b_kstr_ld++) {
18490 cartesian_trajectory_planner_B.obj[cartesian_trajectory_planner_B.b_kstr_ld]
18491 = obj->
18492 JointInternal.JointToParentTransform[cartesian_trajectory_planner_B.b_kstr_ld];
18493 }
18494
18495 for (cartesian_trajectory_planner_B.b_kstr_ld = 0;
18496 cartesian_trajectory_planner_B.b_kstr_ld < 16;
18497 cartesian_trajectory_planner_B.b_kstr_ld++) {
18498 newjoint->JointToParentTransform[cartesian_trajectory_planner_B.b_kstr_ld] =
18499 cartesian_trajectory_planner_B.obj[cartesian_trajectory_planner_B.b_kstr_ld];
18500 }
18501
18502 for (cartesian_trajectory_planner_B.b_kstr_ld = 0;
18503 cartesian_trajectory_planner_B.b_kstr_ld < 16;
18504 cartesian_trajectory_planner_B.b_kstr_ld++) {
18505 cartesian_trajectory_planner_B.obj[cartesian_trajectory_planner_B.b_kstr_ld]
18506 = obj->
18507 JointInternal.ChildToJointTransform[cartesian_trajectory_planner_B.b_kstr_ld];
18508 }
18509
18510 for (cartesian_trajectory_planner_B.b_kstr_ld = 0;
18511 cartesian_trajectory_planner_B.b_kstr_ld < 16;
18512 cartesian_trajectory_planner_B.b_kstr_ld++) {
18513 newjoint->ChildToJointTransform[cartesian_trajectory_planner_B.b_kstr_ld] =
18514 cartesian_trajectory_planner_B.obj[cartesian_trajectory_planner_B.b_kstr_ld];
18515 }
18516
18517 iobj_2->JointInternal = newjoint;
18518 return newbody;
18519}
18520
18521static void cartesian_RigidBodyTree_addBody(x_robotics_manip_internal_Rig_T *obj,
18522 v_robotics_manip_internal_Rig_T *bodyin, const emxArray_char_T_cartesian_tra_T
18523 *parentName, c_rigidBodyJoint_cartesian__a_T *iobj_0,
18524 c_rigidBodyJoint_cartesian__a_T *iobj_1, w_robotics_manip_internal_Rig_T
18525 *iobj_2)
18526{
18527 w_robotics_manip_internal_Rig_T *body;
18528 c_rigidBodyJoint_cartesian__a_T *jnt;
18529 emxArray_char_T_cartesian_tra_T *bname;
18530 static const char_T tmp[5] = { 'f', 'i', 'x', 'e', 'd' };
18531
18532 boolean_T exitg1;
18533 int32_T exitg2;
18534 cartesian_trajec_emxInit_char_T(&bname, 2);
18535 cartesian_trajectory_planner_B.pid = -1.0;
18536 cartesian_trajectory_planner_B.b_kstr_ph = bname->size[0] * bname->size[1];
18537 bname->size[0] = 1;
18538 bname->size[1] = obj->Base.NameInternal->size[1];
18539 cartes_emxEnsureCapacity_char_T(bname,
18540 cartesian_trajectory_planner_B.b_kstr_ph);
18541 cartesian_trajectory_planner_B.loop_ub_ly = obj->Base.NameInternal->size[0] *
18542 obj->Base.NameInternal->size[1] - 1;
18543 for (cartesian_trajectory_planner_B.b_kstr_ph = 0;
18544 cartesian_trajectory_planner_B.b_kstr_ph <=
18545 cartesian_trajectory_planner_B.loop_ub_ly;
18546 cartesian_trajectory_planner_B.b_kstr_ph++) {
18547 bname->data[cartesian_trajectory_planner_B.b_kstr_ph] =
18548 obj->Base.NameInternal->data[cartesian_trajectory_planner_B.b_kstr_ph];
18549 }
18550
18551 if (cartesian_trajectory_pla_strcmp(bname, parentName)) {
18552 cartesian_trajectory_planner_B.pid = 0.0;
18553 } else {
18554 cartesian_trajectory_planner_B.b_index_a = obj->NumBodies;
18555 cartesian_trajectory_planner_B.b_i_po = 0;
18556 exitg1 = false;
18557 while ((!exitg1) && (cartesian_trajectory_planner_B.b_i_po <= static_cast<
18558 int32_T>(cartesian_trajectory_planner_B.b_index_a) - 1))
18559 {
18560 body = obj->Bodies[cartesian_trajectory_planner_B.b_i_po];
18561 cartesian_trajectory_planner_B.b_kstr_ph = bname->size[0] * bname->size[1];
18562 bname->size[0] = 1;
18563 bname->size[1] = body->NameInternal->size[1];
18564 cartes_emxEnsureCapacity_char_T(bname,
18565 cartesian_trajectory_planner_B.b_kstr_ph);
18566 cartesian_trajectory_planner_B.loop_ub_ly = body->NameInternal->size[0] *
18567 body->NameInternal->size[1] - 1;
18568 for (cartesian_trajectory_planner_B.b_kstr_ph = 0;
18569 cartesian_trajectory_planner_B.b_kstr_ph <=
18570 cartesian_trajectory_planner_B.loop_ub_ly;
18571 cartesian_trajectory_planner_B.b_kstr_ph++) {
18572 bname->data[cartesian_trajectory_planner_B.b_kstr_ph] =
18573 body->NameInternal->data[cartesian_trajectory_planner_B.b_kstr_ph];
18574 }
18575
18576 if (cartesian_trajectory_pla_strcmp(bname, parentName)) {
18577 cartesian_trajectory_planner_B.pid = static_cast<real_T>
18578 (cartesian_trajectory_planner_B.b_i_po) + 1.0;
18579 exitg1 = true;
18580 } else {
18581 cartesian_trajectory_planner_B.b_i_po++;
18582 }
18583 }
18584 }
18585
18586 cartesian_trajectory_planner_B.b_index_a = obj->NumBodies + 1.0;
18587 body = cartesian_trajec_RigidBody_copy(bodyin, iobj_1, iobj_0, iobj_2);
18588 obj->Bodies[static_cast<int32_T>(cartesian_trajectory_planner_B.b_index_a) - 1]
18589 = body;
18590 body->Index = cartesian_trajectory_planner_B.b_index_a;
18591 body->ParentIndex = cartesian_trajectory_planner_B.pid;
18592 body->JointInternal->InTree = true;
18593 obj->NumBodies++;
18594 jnt = body->JointInternal;
18595 cartesian_trajectory_planner_B.b_kstr_ph = bname->size[0] * bname->size[1];
18596 bname->size[0] = 1;
18597 bname->size[1] = jnt->Type->size[1];
18598 cartes_emxEnsureCapacity_char_T(bname,
18599 cartesian_trajectory_planner_B.b_kstr_ph);
18600 cartesian_trajectory_planner_B.loop_ub_ly = jnt->Type->size[0] * jnt->
18601 Type->size[1] - 1;
18602 for (cartesian_trajectory_planner_B.b_kstr_ph = 0;
18603 cartesian_trajectory_planner_B.b_kstr_ph <=
18604 cartesian_trajectory_planner_B.loop_ub_ly;
18605 cartesian_trajectory_planner_B.b_kstr_ph++) {
18606 bname->data[cartesian_trajectory_planner_B.b_kstr_ph] = jnt->Type->
18607 data[cartesian_trajectory_planner_B.b_kstr_ph];
18608 }
18609
18610 for (cartesian_trajectory_planner_B.b_kstr_ph = 0;
18611 cartesian_trajectory_planner_B.b_kstr_ph < 5;
18612 cartesian_trajectory_planner_B.b_kstr_ph++) {
18613 cartesian_trajectory_planner_B.b_b[cartesian_trajectory_planner_B.b_kstr_ph]
18614 = tmp[cartesian_trajectory_planner_B.b_kstr_ph];
18615 }
18616
18617 cartesian_trajectory_planner_B.b_bool_fd = false;
18618 if (bname->size[1] == 5) {
18619 cartesian_trajectory_planner_B.b_kstr_ph = 1;
18620 do {
18621 exitg2 = 0;
18622 if (cartesian_trajectory_planner_B.b_kstr_ph - 1 < 5) {
18623 cartesian_trajectory_planner_B.loop_ub_ly =
18624 cartesian_trajectory_planner_B.b_kstr_ph - 1;
18625 if (bname->data[cartesian_trajectory_planner_B.loop_ub_ly] !=
18626 cartesian_trajectory_planner_B.b_b[cartesian_trajectory_planner_B.loop_ub_ly])
18627 {
18628 exitg2 = 1;
18629 } else {
18630 cartesian_trajectory_planner_B.b_kstr_ph++;
18631 }
18632 } else {
18633 cartesian_trajectory_planner_B.b_bool_fd = true;
18634 exitg2 = 1;
18635 }
18636 } while (exitg2 == 0);
18637 }
18638
18639 cartesian_trajec_emxFree_char_T(&bname);
18640 if (!cartesian_trajectory_planner_B.b_bool_fd) {
18641 obj->NumNonFixedBodies++;
18642 jnt = body->JointInternal;
18643 cartesian_trajectory_planner_B.b_kstr_ph = static_cast<int32_T>(body->Index)
18644 - 1;
18645 obj->PositionDoFMap[cartesian_trajectory_planner_B.b_kstr_ph] =
18646 obj->PositionNumber + 1.0;
18647 obj->PositionDoFMap[cartesian_trajectory_planner_B.b_kstr_ph + 8] =
18648 obj->PositionNumber + jnt->PositionNumber;
18649 jnt = body->JointInternal;
18650 cartesian_trajectory_planner_B.b_kstr_ph = static_cast<int32_T>(body->Index)
18651 - 1;
18652 obj->VelocityDoFMap[cartesian_trajectory_planner_B.b_kstr_ph] =
18653 obj->VelocityNumber + 1.0;
18654 obj->VelocityDoFMap[cartesian_trajectory_planner_B.b_kstr_ph + 8] =
18655 obj->VelocityNumber + jnt->VelocityNumber;
18656 } else {
18657 cartesian_trajectory_planner_B.b_kstr_ph = static_cast<int32_T>(body->Index);
18658 obj->PositionDoFMap[cartesian_trajectory_planner_B.b_kstr_ph - 1] = 0.0;
18659 obj->PositionDoFMap[cartesian_trajectory_planner_B.b_kstr_ph + 7] = -1.0;
18660 cartesian_trajectory_planner_B.b_kstr_ph = static_cast<int32_T>(body->Index);
18661 obj->VelocityDoFMap[cartesian_trajectory_planner_B.b_kstr_ph - 1] = 0.0;
18662 obj->VelocityDoFMap[cartesian_trajectory_planner_B.b_kstr_ph + 7] = -1.0;
18663 }
18664
18665 jnt = body->JointInternal;
18666 obj->PositionNumber += jnt->PositionNumber;
18667 jnt = body->JointInternal;
18668 obj->VelocityNumber += jnt->VelocityNumber;
18669}
18670
18671static void inverseKinematics_set_RigidBody(b_inverseKinematics_cartesian_T *obj,
18672 y_robotics_manip_internal_Rig_T *rigidbodytree,
18673 w_robotics_manip_internal_Rig_T *iobj_0, w_robotics_manip_internal_Rig_T
18674 *iobj_1, w_robotics_manip_internal_Rig_T *iobj_2,
18675 w_robotics_manip_internal_Rig_T *iobj_3, w_robotics_manip_internal_Rig_T
18676 *iobj_4, w_robotics_manip_internal_Rig_T *iobj_5,
18677 w_robotics_manip_internal_Rig_T *iobj_6, w_robotics_manip_internal_Rig_T
18678 *iobj_7, w_robotics_manip_internal_Rig_T *iobj_8,
18679 w_robotics_manip_internal_Rig_T *iobj_9, w_robotics_manip_internal_Rig_T
18680 *iobj_10, w_robotics_manip_internal_Rig_T *iobj_11,
18681 w_robotics_manip_internal_Rig_T *iobj_12, w_robotics_manip_internal_Rig_T
18682 *iobj_13, w_robotics_manip_internal_Rig_T *iobj_14,
18683 c_rigidBodyJoint_cartesian__a_T *iobj_15, c_rigidBodyJoint_cartesian__a_T
18684 *iobj_16, c_rigidBodyJoint_cartesian__a_T *iobj_17,
18685 c_rigidBodyJoint_cartesian__a_T *iobj_18, c_rigidBodyJoint_cartesian__a_T
18686 *iobj_19, c_rigidBodyJoint_cartesian__a_T *iobj_20,
18687 c_rigidBodyJoint_cartesian__a_T *iobj_21, c_rigidBodyJoint_cartesian__a_T
18688 *iobj_22, c_rigidBodyJoint_cartesian__a_T *iobj_23,
18689 c_rigidBodyJoint_cartesian__a_T *iobj_24, c_rigidBodyJoint_cartesian__a_T
18690 *iobj_25, c_rigidBodyJoint_cartesian__a_T *iobj_26,
18691 c_rigidBodyJoint_cartesian__a_T *iobj_27, c_rigidBodyJoint_cartesian__a_T
18692 *iobj_28, c_rigidBodyJoint_cartesian__a_T *iobj_29,
18693 c_rigidBodyJoint_cartesian__a_T *iobj_30, c_rigidBodyJoint_cartesian__a_T
18694 *iobj_31, c_rigidBodyJoint_cartesian__a_T *iobj_32,
18695 c_rigidBodyJoint_cartesian__a_T *iobj_33, c_rigidBodyJoint_cartesian__a_T
18696 *iobj_34, c_rigidBodyJoint_cartesian__a_T *iobj_35,
18697 c_rigidBodyJoint_cartesian__a_T *iobj_36, c_rigidBodyJoint_cartesian__a_T
18698 *iobj_37, c_rigidBodyJoint_cartesian__a_T *iobj_38,
18699 c_rigidBodyJoint_cartesian__a_T *iobj_39, w_robotics_manip_internal_Rig_T
18700 *iobj_40, x_robotics_manip_internal_Rig_T *iobj_41)
18701{
18702 x_robotics_manip_internal_Rig_T *newrobot;
18703 v_robotics_manip_internal_Rig_T *body;
18704 v_robotics_manip_internal_Rig_T *parent;
18705 emxArray_char_T_cartesian_tra_T *b_basename;
18706 w_robotics_manip_internal_Rig_T *body_0;
18707 c_rigidBodyJoint_cartesian__a_T *jnt;
18708 emxArray_char_T_cartesian_tra_T *bname;
18709 static const char_T tmp[5] = { 'f', 'i', 'x', 'e', 'd' };
18710
18711 boolean_T exitg1;
18712 int32_T exitg2;
18713 cartesian_trajec_emxInit_char_T(&b_basename, 2);
18714 newrobot = RigidBodyTree_RigidBodyTree_as(iobj_41, iobj_0, iobj_1, iobj_2,
18715 iobj_3, iobj_4, iobj_5, iobj_6, iobj_15, iobj_16, iobj_17, iobj_18, iobj_19,
18716 iobj_20, iobj_21, iobj_22, iobj_39, iobj_40);
18717 cartesian_trajectory_planner_B.b_kstr_g = b_basename->size[0] *
18718 b_basename->size[1];
18719 b_basename->size[0] = 1;
18720 b_basename->size[1] = rigidbodytree->Base.NameInternal->size[1];
18721 cartes_emxEnsureCapacity_char_T(b_basename,
18722 cartesian_trajectory_planner_B.b_kstr_g);
18723 cartesian_trajectory_planner_B.loop_ub_j = rigidbodytree->
18724 Base.NameInternal->size[0] * rigidbodytree->Base.NameInternal->size[1] - 1;
18725 for (cartesian_trajectory_planner_B.b_kstr_g = 0;
18726 cartesian_trajectory_planner_B.b_kstr_g <=
18727 cartesian_trajectory_planner_B.loop_ub_j;
18728 cartesian_trajectory_planner_B.b_kstr_g++) {
18729 b_basename->data[cartesian_trajectory_planner_B.b_kstr_g] =
18730 rigidbodytree->Base.NameInternal->
18731 data[cartesian_trajectory_planner_B.b_kstr_g];
18732 }
18733
18734 cartesian_trajec_emxInit_char_T(&bname, 2);
18735 cartesian_trajectory_planner_B.bid_c = -1.0;
18736 cartesian_trajectory_planner_B.b_kstr_g = bname->size[0] * bname->size[1];
18737 bname->size[0] = 1;
18738 bname->size[1] = newrobot->Base.NameInternal->size[1];
18739 cartes_emxEnsureCapacity_char_T(bname, cartesian_trajectory_planner_B.b_kstr_g);
18740 cartesian_trajectory_planner_B.loop_ub_j = newrobot->Base.NameInternal->size[0]
18741 * newrobot->Base.NameInternal->size[1] - 1;
18742 for (cartesian_trajectory_planner_B.b_kstr_g = 0;
18743 cartesian_trajectory_planner_B.b_kstr_g <=
18744 cartesian_trajectory_planner_B.loop_ub_j;
18745 cartesian_trajectory_planner_B.b_kstr_g++) {
18746 bname->data[cartesian_trajectory_planner_B.b_kstr_g] =
18747 newrobot->Base.NameInternal->data[cartesian_trajectory_planner_B.b_kstr_g];
18748 }
18749
18750 if (cartesian_trajectory_pla_strcmp(bname, b_basename)) {
18751 cartesian_trajectory_planner_B.bid_c = 0.0;
18752 } else {
18753 cartesian_trajectory_planner_B.b_index = newrobot->NumBodies;
18754 cartesian_trajectory_planner_B.b_i_ft = 0;
18755 exitg1 = false;
18756 while ((!exitg1) && (cartesian_trajectory_planner_B.b_i_ft <=
18757 static_cast<int32_T>
18758 (cartesian_trajectory_planner_B.b_index) - 1)) {
18759 body_0 = newrobot->Bodies[cartesian_trajectory_planner_B.b_i_ft];
18760 cartesian_trajectory_planner_B.b_kstr_g = bname->size[0] * bname->size[1];
18761 bname->size[0] = 1;
18762 bname->size[1] = body_0->NameInternal->size[1];
18763 cartes_emxEnsureCapacity_char_T(bname,
18764 cartesian_trajectory_planner_B.b_kstr_g);
18765 cartesian_trajectory_planner_B.loop_ub_j = body_0->NameInternal->size[0] *
18766 body_0->NameInternal->size[1] - 1;
18767 for (cartesian_trajectory_planner_B.b_kstr_g = 0;
18768 cartesian_trajectory_planner_B.b_kstr_g <=
18769 cartesian_trajectory_planner_B.loop_ub_j;
18770 cartesian_trajectory_planner_B.b_kstr_g++) {
18771 bname->data[cartesian_trajectory_planner_B.b_kstr_g] =
18772 body_0->NameInternal->data[cartesian_trajectory_planner_B.b_kstr_g];
18773 }
18774
18775 if (cartesian_trajectory_pla_strcmp(bname, b_basename)) {
18776 cartesian_trajectory_planner_B.bid_c = static_cast<real_T>
18777 (cartesian_trajectory_planner_B.b_i_ft) + 1.0;
18778 exitg1 = true;
18779 } else {
18780 cartesian_trajectory_planner_B.b_i_ft++;
18781 }
18782 }
18783 }
18784
18785 if ((!(cartesian_trajectory_planner_B.bid_c == 0.0)) &&
18786 (cartesian_trajectory_planner_B.bid_c < 0.0)) {
18787 cartesian_trajectory_planner_B.b_kstr_g = newrobot->Base.NameInternal->size
18788 [0] * newrobot->Base.NameInternal->size[1];
18789 newrobot->Base.NameInternal->size[0] = 1;
18790 newrobot->Base.NameInternal->size[1] = b_basename->size[1];
18791 cartes_emxEnsureCapacity_char_T(newrobot->Base.NameInternal,
18792 cartesian_trajectory_planner_B.b_kstr_g);
18793 cartesian_trajectory_planner_B.loop_ub_j = b_basename->size[0] *
18794 b_basename->size[1] - 1;
18795 for (cartesian_trajectory_planner_B.b_kstr_g = 0;
18796 cartesian_trajectory_planner_B.b_kstr_g <=
18797 cartesian_trajectory_planner_B.loop_ub_j;
18798 cartesian_trajectory_planner_B.b_kstr_g++) {
18799 newrobot->Base.NameInternal->data[cartesian_trajectory_planner_B.b_kstr_g]
18800 = b_basename->data[cartesian_trajectory_planner_B.b_kstr_g];
18801 }
18802 }
18803
18804 if (1.0 <= rigidbodytree->NumBodies) {
18805 body = rigidbodytree->Bodies[0];
18806 cartesian_trajectory_planner_B.bid_c = body->ParentIndex;
18807 if (cartesian_trajectory_planner_B.bid_c > 0.0) {
18808 parent = rigidbodytree->Bodies[static_cast<int32_T>
18809 (cartesian_trajectory_planner_B.bid_c) - 1];
18810 } else {
18811 parent = &rigidbodytree->Base;
18812 }
18813
18814 cartesian_trajectory_planner_B.b_kstr_g = bname->size[0] * bname->size[1];
18815 bname->size[0] = 1;
18816 bname->size[1] = parent->NameInternal->size[1];
18817 cartes_emxEnsureCapacity_char_T(bname,
18818 cartesian_trajectory_planner_B.b_kstr_g);
18819 cartesian_trajectory_planner_B.loop_ub_j = parent->NameInternal->size[0] *
18820 parent->NameInternal->size[1] - 1;
18821 for (cartesian_trajectory_planner_B.b_kstr_g = 0;
18822 cartesian_trajectory_planner_B.b_kstr_g <=
18823 cartesian_trajectory_planner_B.loop_ub_j;
18824 cartesian_trajectory_planner_B.b_kstr_g++) {
18825 bname->data[cartesian_trajectory_planner_B.b_kstr_g] =
18826 parent->NameInternal->data[cartesian_trajectory_planner_B.b_kstr_g];
18827 }
18828
18829 cartesian_RigidBodyTree_addBody(newrobot, body, bname, iobj_24, iobj_23,
18830 iobj_7);
18831 }
18832
18833 if (2.0 <= rigidbodytree->NumBodies) {
18834 body = rigidbodytree->Bodies[1];
18835 cartesian_trajectory_planner_B.bid_c = body->ParentIndex;
18836 if (cartesian_trajectory_planner_B.bid_c > 0.0) {
18837 parent = rigidbodytree->Bodies[static_cast<int32_T>
18838 (cartesian_trajectory_planner_B.bid_c) - 1];
18839 } else {
18840 parent = &rigidbodytree->Base;
18841 }
18842
18843 cartesian_trajectory_planner_B.b_kstr_g = bname->size[0] * bname->size[1];
18844 bname->size[0] = 1;
18845 bname->size[1] = parent->NameInternal->size[1];
18846 cartes_emxEnsureCapacity_char_T(bname,
18847 cartesian_trajectory_planner_B.b_kstr_g);
18848 cartesian_trajectory_planner_B.loop_ub_j = parent->NameInternal->size[0] *
18849 parent->NameInternal->size[1] - 1;
18850 for (cartesian_trajectory_planner_B.b_kstr_g = 0;
18851 cartesian_trajectory_planner_B.b_kstr_g <=
18852 cartesian_trajectory_planner_B.loop_ub_j;
18853 cartesian_trajectory_planner_B.b_kstr_g++) {
18854 bname->data[cartesian_trajectory_planner_B.b_kstr_g] =
18855 parent->NameInternal->data[cartesian_trajectory_planner_B.b_kstr_g];
18856 }
18857
18858 cartesian_RigidBodyTree_addBody(newrobot, body, bname, iobj_26, iobj_25,
18859 iobj_8);
18860 }
18861
18862 if (3.0 <= rigidbodytree->NumBodies) {
18863 body = rigidbodytree->Bodies[2];
18864 cartesian_trajectory_planner_B.bid_c = body->ParentIndex;
18865 if (cartesian_trajectory_planner_B.bid_c > 0.0) {
18866 parent = rigidbodytree->Bodies[static_cast<int32_T>
18867 (cartesian_trajectory_planner_B.bid_c) - 1];
18868 } else {
18869 parent = &rigidbodytree->Base;
18870 }
18871
18872 cartesian_trajectory_planner_B.b_kstr_g = bname->size[0] * bname->size[1];
18873 bname->size[0] = 1;
18874 bname->size[1] = parent->NameInternal->size[1];
18875 cartes_emxEnsureCapacity_char_T(bname,
18876 cartesian_trajectory_planner_B.b_kstr_g);
18877 cartesian_trajectory_planner_B.loop_ub_j = parent->NameInternal->size[0] *
18878 parent->NameInternal->size[1] - 1;
18879 for (cartesian_trajectory_planner_B.b_kstr_g = 0;
18880 cartesian_trajectory_planner_B.b_kstr_g <=
18881 cartesian_trajectory_planner_B.loop_ub_j;
18882 cartesian_trajectory_planner_B.b_kstr_g++) {
18883 bname->data[cartesian_trajectory_planner_B.b_kstr_g] =
18884 parent->NameInternal->data[cartesian_trajectory_planner_B.b_kstr_g];
18885 }
18886
18887 cartesian_RigidBodyTree_addBody(newrobot, body, bname, iobj_28, iobj_27,
18888 iobj_9);
18889 }
18890
18891 if (4.0 <= rigidbodytree->NumBodies) {
18892 body = rigidbodytree->Bodies[3];
18893 cartesian_trajectory_planner_B.bid_c = body->ParentIndex;
18894 if (cartesian_trajectory_planner_B.bid_c > 0.0) {
18895 parent = rigidbodytree->Bodies[static_cast<int32_T>
18896 (cartesian_trajectory_planner_B.bid_c) - 1];
18897 } else {
18898 parent = &rigidbodytree->Base;
18899 }
18900
18901 cartesian_trajectory_planner_B.b_kstr_g = bname->size[0] * bname->size[1];
18902 bname->size[0] = 1;
18903 bname->size[1] = parent->NameInternal->size[1];
18904 cartes_emxEnsureCapacity_char_T(bname,
18905 cartesian_trajectory_planner_B.b_kstr_g);
18906 cartesian_trajectory_planner_B.loop_ub_j = parent->NameInternal->size[0] *
18907 parent->NameInternal->size[1] - 1;
18908 for (cartesian_trajectory_planner_B.b_kstr_g = 0;
18909 cartesian_trajectory_planner_B.b_kstr_g <=
18910 cartesian_trajectory_planner_B.loop_ub_j;
18911 cartesian_trajectory_planner_B.b_kstr_g++) {
18912 bname->data[cartesian_trajectory_planner_B.b_kstr_g] =
18913 parent->NameInternal->data[cartesian_trajectory_planner_B.b_kstr_g];
18914 }
18915
18916 cartesian_RigidBodyTree_addBody(newrobot, body, bname, iobj_30, iobj_29,
18917 iobj_10);
18918 }
18919
18920 if (5.0 <= rigidbodytree->NumBodies) {
18921 body = rigidbodytree->Bodies[4];
18922 cartesian_trajectory_planner_B.bid_c = body->ParentIndex;
18923 if (cartesian_trajectory_planner_B.bid_c > 0.0) {
18924 parent = rigidbodytree->Bodies[static_cast<int32_T>
18925 (cartesian_trajectory_planner_B.bid_c) - 1];
18926 } else {
18927 parent = &rigidbodytree->Base;
18928 }
18929
18930 cartesian_trajectory_planner_B.b_kstr_g = bname->size[0] * bname->size[1];
18931 bname->size[0] = 1;
18932 bname->size[1] = parent->NameInternal->size[1];
18933 cartes_emxEnsureCapacity_char_T(bname,
18934 cartesian_trajectory_planner_B.b_kstr_g);
18935 cartesian_trajectory_planner_B.loop_ub_j = parent->NameInternal->size[0] *
18936 parent->NameInternal->size[1] - 1;
18937 for (cartesian_trajectory_planner_B.b_kstr_g = 0;
18938 cartesian_trajectory_planner_B.b_kstr_g <=
18939 cartesian_trajectory_planner_B.loop_ub_j;
18940 cartesian_trajectory_planner_B.b_kstr_g++) {
18941 bname->data[cartesian_trajectory_planner_B.b_kstr_g] =
18942 parent->NameInternal->data[cartesian_trajectory_planner_B.b_kstr_g];
18943 }
18944
18945 cartesian_RigidBodyTree_addBody(newrobot, body, bname, iobj_32, iobj_31,
18946 iobj_11);
18947 }
18948
18949 if (6.0 <= rigidbodytree->NumBodies) {
18950 body = rigidbodytree->Bodies[5];
18951 cartesian_trajectory_planner_B.bid_c = body->ParentIndex;
18952 if (cartesian_trajectory_planner_B.bid_c > 0.0) {
18953 parent = rigidbodytree->Bodies[static_cast<int32_T>
18954 (cartesian_trajectory_planner_B.bid_c) - 1];
18955 } else {
18956 parent = &rigidbodytree->Base;
18957 }
18958
18959 cartesian_trajectory_planner_B.b_kstr_g = bname->size[0] * bname->size[1];
18960 bname->size[0] = 1;
18961 bname->size[1] = parent->NameInternal->size[1];
18962 cartes_emxEnsureCapacity_char_T(bname,
18963 cartesian_trajectory_planner_B.b_kstr_g);
18964 cartesian_trajectory_planner_B.loop_ub_j = parent->NameInternal->size[0] *
18965 parent->NameInternal->size[1] - 1;
18966 for (cartesian_trajectory_planner_B.b_kstr_g = 0;
18967 cartesian_trajectory_planner_B.b_kstr_g <=
18968 cartesian_trajectory_planner_B.loop_ub_j;
18969 cartesian_trajectory_planner_B.b_kstr_g++) {
18970 bname->data[cartesian_trajectory_planner_B.b_kstr_g] =
18971 parent->NameInternal->data[cartesian_trajectory_planner_B.b_kstr_g];
18972 }
18973
18974 cartesian_RigidBodyTree_addBody(newrobot, body, bname, iobj_34, iobj_33,
18975 iobj_12);
18976 }
18977
18978 if (7.0 <= rigidbodytree->NumBodies) {
18979 body = rigidbodytree->Bodies[6];
18980 cartesian_trajectory_planner_B.bid_c = body->ParentIndex;
18981 if (cartesian_trajectory_planner_B.bid_c > 0.0) {
18982 parent = rigidbodytree->Bodies[static_cast<int32_T>
18983 (cartesian_trajectory_planner_B.bid_c) - 1];
18984 } else {
18985 parent = &rigidbodytree->Base;
18986 }
18987
18988 cartesian_trajectory_planner_B.b_kstr_g = bname->size[0] * bname->size[1];
18989 bname->size[0] = 1;
18990 bname->size[1] = parent->NameInternal->size[1];
18991 cartes_emxEnsureCapacity_char_T(bname,
18992 cartesian_trajectory_planner_B.b_kstr_g);
18993 cartesian_trajectory_planner_B.loop_ub_j = parent->NameInternal->size[0] *
18994 parent->NameInternal->size[1] - 1;
18995 for (cartesian_trajectory_planner_B.b_kstr_g = 0;
18996 cartesian_trajectory_planner_B.b_kstr_g <=
18997 cartesian_trajectory_planner_B.loop_ub_j;
18998 cartesian_trajectory_planner_B.b_kstr_g++) {
18999 bname->data[cartesian_trajectory_planner_B.b_kstr_g] =
19000 parent->NameInternal->data[cartesian_trajectory_planner_B.b_kstr_g];
19001 }
19002
19003 cartesian_trajectory_planner_B.bid_c = -1.0;
19004 cartesian_trajectory_planner_B.b_kstr_g = b_basename->size[0] *
19005 b_basename->size[1];
19006 b_basename->size[0] = 1;
19007 b_basename->size[1] = newrobot->Base.NameInternal->size[1];
19008 cartes_emxEnsureCapacity_char_T(b_basename,
19009 cartesian_trajectory_planner_B.b_kstr_g);
19010 cartesian_trajectory_planner_B.loop_ub_j = newrobot->Base.NameInternal->
19011 size[0] * newrobot->Base.NameInternal->size[1] - 1;
19012 for (cartesian_trajectory_planner_B.b_kstr_g = 0;
19013 cartesian_trajectory_planner_B.b_kstr_g <=
19014 cartesian_trajectory_planner_B.loop_ub_j;
19015 cartesian_trajectory_planner_B.b_kstr_g++) {
19016 b_basename->data[cartesian_trajectory_planner_B.b_kstr_g] =
19017 newrobot->Base.NameInternal->
19018 data[cartesian_trajectory_planner_B.b_kstr_g];
19019 }
19020
19021 if (cartesian_trajectory_pla_strcmp(b_basename, bname)) {
19022 cartesian_trajectory_planner_B.bid_c = 0.0;
19023 } else {
19024 cartesian_trajectory_planner_B.b_index = newrobot->NumBodies;
19025 cartesian_trajectory_planner_B.b_i_ft = 0;
19026 exitg1 = false;
19027 while ((!exitg1) && (cartesian_trajectory_planner_B.b_i_ft <=
19028 static_cast<int32_T>
19029 (cartesian_trajectory_planner_B.b_index) - 1)) {
19030 body_0 = newrobot->Bodies[cartesian_trajectory_planner_B.b_i_ft];
19031 cartesian_trajectory_planner_B.b_kstr_g = b_basename->size[0] *
19032 b_basename->size[1];
19033 b_basename->size[0] = 1;
19034 b_basename->size[1] = body_0->NameInternal->size[1];
19035 cartes_emxEnsureCapacity_char_T(b_basename,
19036 cartesian_trajectory_planner_B.b_kstr_g);
19037 cartesian_trajectory_planner_B.loop_ub_j = body_0->NameInternal->size[0]
19038 * body_0->NameInternal->size[1] - 1;
19039 for (cartesian_trajectory_planner_B.b_kstr_g = 0;
19040 cartesian_trajectory_planner_B.b_kstr_g <=
19041 cartesian_trajectory_planner_B.loop_ub_j;
19042 cartesian_trajectory_planner_B.b_kstr_g++) {
19043 b_basename->data[cartesian_trajectory_planner_B.b_kstr_g] =
19044 body_0->NameInternal->data[cartesian_trajectory_planner_B.b_kstr_g];
19045 }
19046
19047 if (cartesian_trajectory_pla_strcmp(b_basename, bname)) {
19048 cartesian_trajectory_planner_B.bid_c = static_cast<real_T>
19049 (cartesian_trajectory_planner_B.b_i_ft) + 1.0;
19050 exitg1 = true;
19051 } else {
19052 cartesian_trajectory_planner_B.b_i_ft++;
19053 }
19054 }
19055 }
19056
19057 cartesian_trajectory_planner_B.b_index = newrobot->NumBodies + 1.0;
19058 body_0 = cartesian_trajec_RigidBody_copy(body, iobj_35, iobj_36, iobj_13);
19059 newrobot->Bodies[static_cast<int32_T>(cartesian_trajectory_planner_B.b_index)
19060 - 1] = body_0;
19061 body_0->Index = cartesian_trajectory_planner_B.b_index;
19062 body_0->ParentIndex = cartesian_trajectory_planner_B.bid_c;
19063 body_0->JointInternal->InTree = true;
19064 newrobot->NumBodies++;
19065 jnt = body_0->JointInternal;
19066 cartesian_trajectory_planner_B.b_kstr_g = bname->size[0] * bname->size[1];
19067 bname->size[0] = 1;
19068 bname->size[1] = jnt->Type->size[1];
19069 cartes_emxEnsureCapacity_char_T(bname,
19070 cartesian_trajectory_planner_B.b_kstr_g);
19071 cartesian_trajectory_planner_B.loop_ub_j = jnt->Type->size[0] * jnt->
19072 Type->size[1] - 1;
19073 for (cartesian_trajectory_planner_B.b_kstr_g = 0;
19074 cartesian_trajectory_planner_B.b_kstr_g <=
19075 cartesian_trajectory_planner_B.loop_ub_j;
19076 cartesian_trajectory_planner_B.b_kstr_g++) {
19077 bname->data[cartesian_trajectory_planner_B.b_kstr_g] = jnt->Type->
19078 data[cartesian_trajectory_planner_B.b_kstr_g];
19079 }
19080
19081 for (cartesian_trajectory_planner_B.b_kstr_g = 0;
19082 cartesian_trajectory_planner_B.b_kstr_g < 5;
19083 cartesian_trajectory_planner_B.b_kstr_g++) {
19084 cartesian_trajectory_planner_B.b_js[cartesian_trajectory_planner_B.b_kstr_g]
19085 = tmp[cartesian_trajectory_planner_B.b_kstr_g];
19086 }
19087
19088 cartesian_trajectory_planner_B.b_bool_p = false;
19089 if (bname->size[1] == 5) {
19090 cartesian_trajectory_planner_B.b_kstr_g = 1;
19091 do {
19092 exitg2 = 0;
19093 if (cartesian_trajectory_planner_B.b_kstr_g - 1 < 5) {
19094 cartesian_trajectory_planner_B.loop_ub_j =
19095 cartesian_trajectory_planner_B.b_kstr_g - 1;
19096 if (bname->data[cartesian_trajectory_planner_B.loop_ub_j] !=
19097 cartesian_trajectory_planner_B.b_js[cartesian_trajectory_planner_B.loop_ub_j])
19098 {
19099 exitg2 = 1;
19100 } else {
19101 cartesian_trajectory_planner_B.b_kstr_g++;
19102 }
19103 } else {
19104 cartesian_trajectory_planner_B.b_bool_p = true;
19105 exitg2 = 1;
19106 }
19107 } while (exitg2 == 0);
19108 }
19109
19110 if (!cartesian_trajectory_planner_B.b_bool_p) {
19111 newrobot->NumNonFixedBodies++;
19112 jnt = body_0->JointInternal;
19113 cartesian_trajectory_planner_B.b_kstr_g = static_cast<int32_T>
19114 (body_0->Index) - 1;
19115 newrobot->PositionDoFMap[cartesian_trajectory_planner_B.b_kstr_g] =
19116 newrobot->PositionNumber + 1.0;
19117 newrobot->PositionDoFMap[cartesian_trajectory_planner_B.b_kstr_g + 8] =
19118 newrobot->PositionNumber + jnt->PositionNumber;
19119 jnt = body_0->JointInternal;
19120 cartesian_trajectory_planner_B.b_kstr_g = static_cast<int32_T>
19121 (body_0->Index) - 1;
19122 newrobot->VelocityDoFMap[cartesian_trajectory_planner_B.b_kstr_g] =
19123 newrobot->VelocityNumber + 1.0;
19124 newrobot->VelocityDoFMap[cartesian_trajectory_planner_B.b_kstr_g + 8] =
19125 newrobot->VelocityNumber + jnt->VelocityNumber;
19126 } else {
19127 cartesian_trajectory_planner_B.b_kstr_g = static_cast<int32_T>
19128 (body_0->Index);
19129 newrobot->PositionDoFMap[cartesian_trajectory_planner_B.b_kstr_g - 1] =
19130 0.0;
19131 newrobot->PositionDoFMap[cartesian_trajectory_planner_B.b_kstr_g + 7] =
19132 -1.0;
19133 cartesian_trajectory_planner_B.b_kstr_g = static_cast<int32_T>
19134 (body_0->Index);
19135 newrobot->VelocityDoFMap[cartesian_trajectory_planner_B.b_kstr_g - 1] =
19136 0.0;
19137 newrobot->VelocityDoFMap[cartesian_trajectory_planner_B.b_kstr_g + 7] =
19138 -1.0;
19139 }
19140
19141 jnt = body_0->JointInternal;
19142 newrobot->PositionNumber += jnt->PositionNumber;
19143 jnt = body_0->JointInternal;
19144 newrobot->VelocityNumber += jnt->VelocityNumber;
19145 }
19146
19147 if (8.0 <= rigidbodytree->NumBodies) {
19148 body = rigidbodytree->Bodies[7];
19149 cartesian_trajectory_planner_B.bid_c = body->ParentIndex;
19150 if (cartesian_trajectory_planner_B.bid_c > 0.0) {
19151 parent = rigidbodytree->Bodies[static_cast<int32_T>
19152 (cartesian_trajectory_planner_B.bid_c) - 1];
19153 } else {
19154 parent = &rigidbodytree->Base;
19155 }
19156
19157 cartesian_trajectory_planner_B.b_kstr_g = bname->size[0] * bname->size[1];
19158 bname->size[0] = 1;
19159 bname->size[1] = parent->NameInternal->size[1];
19160 cartes_emxEnsureCapacity_char_T(bname,
19161 cartesian_trajectory_planner_B.b_kstr_g);
19162 cartesian_trajectory_planner_B.loop_ub_j = parent->NameInternal->size[0] *
19163 parent->NameInternal->size[1] - 1;
19164 for (cartesian_trajectory_planner_B.b_kstr_g = 0;
19165 cartesian_trajectory_planner_B.b_kstr_g <=
19166 cartesian_trajectory_planner_B.loop_ub_j;
19167 cartesian_trajectory_planner_B.b_kstr_g++) {
19168 bname->data[cartesian_trajectory_planner_B.b_kstr_g] =
19169 parent->NameInternal->data[cartesian_trajectory_planner_B.b_kstr_g];
19170 }
19171
19172 cartesian_trajectory_planner_B.bid_c = -1.0;
19173 cartesian_trajectory_planner_B.b_kstr_g = b_basename->size[0] *
19174 b_basename->size[1];
19175 b_basename->size[0] = 1;
19176 b_basename->size[1] = newrobot->Base.NameInternal->size[1];
19177 cartes_emxEnsureCapacity_char_T(b_basename,
19178 cartesian_trajectory_planner_B.b_kstr_g);
19179 cartesian_trajectory_planner_B.loop_ub_j = newrobot->Base.NameInternal->
19180 size[0] * newrobot->Base.NameInternal->size[1] - 1;
19181 for (cartesian_trajectory_planner_B.b_kstr_g = 0;
19182 cartesian_trajectory_planner_B.b_kstr_g <=
19183 cartesian_trajectory_planner_B.loop_ub_j;
19184 cartesian_trajectory_planner_B.b_kstr_g++) {
19185 b_basename->data[cartesian_trajectory_planner_B.b_kstr_g] =
19186 newrobot->Base.NameInternal->
19187 data[cartesian_trajectory_planner_B.b_kstr_g];
19188 }
19189
19190 if (cartesian_trajectory_pla_strcmp(b_basename, bname)) {
19191 cartesian_trajectory_planner_B.bid_c = 0.0;
19192 } else {
19193 cartesian_trajectory_planner_B.b_index = newrobot->NumBodies;
19194 cartesian_trajectory_planner_B.b_i_ft = 0;
19195 exitg1 = false;
19196 while ((!exitg1) && (cartesian_trajectory_planner_B.b_i_ft <= static_cast<
19197 int32_T>(cartesian_trajectory_planner_B.b_index) - 1))
19198 {
19199 body_0 = newrobot->Bodies[cartesian_trajectory_planner_B.b_i_ft];
19200 cartesian_trajectory_planner_B.b_kstr_g = b_basename->size[0] *
19201 b_basename->size[1];
19202 b_basename->size[0] = 1;
19203 b_basename->size[1] = body_0->NameInternal->size[1];
19204 cartes_emxEnsureCapacity_char_T(b_basename,
19205 cartesian_trajectory_planner_B.b_kstr_g);
19206 cartesian_trajectory_planner_B.loop_ub_j = body_0->NameInternal->size[0]
19207 * body_0->NameInternal->size[1] - 1;
19208 for (cartesian_trajectory_planner_B.b_kstr_g = 0;
19209 cartesian_trajectory_planner_B.b_kstr_g <=
19210 cartesian_trajectory_planner_B.loop_ub_j;
19211 cartesian_trajectory_planner_B.b_kstr_g++) {
19212 b_basename->data[cartesian_trajectory_planner_B.b_kstr_g] =
19213 body_0->NameInternal->data[cartesian_trajectory_planner_B.b_kstr_g];
19214 }
19215
19216 if (cartesian_trajectory_pla_strcmp(b_basename, bname)) {
19217 cartesian_trajectory_planner_B.bid_c = static_cast<real_T>
19218 (cartesian_trajectory_planner_B.b_i_ft) + 1.0;
19219 exitg1 = true;
19220 } else {
19221 cartesian_trajectory_planner_B.b_i_ft++;
19222 }
19223 }
19224 }
19225
19226 cartesian_trajectory_planner_B.b_index = newrobot->NumBodies + 1.0;
19227 body_0 = cartesian_trajec_RigidBody_copy(body, iobj_37, iobj_38, iobj_14);
19228 newrobot->Bodies[static_cast<int32_T>(cartesian_trajectory_planner_B.b_index)
19229 - 1] = body_0;
19230 body_0->Index = cartesian_trajectory_planner_B.b_index;
19231 body_0->ParentIndex = cartesian_trajectory_planner_B.bid_c;
19232 body_0->JointInternal->InTree = true;
19233 newrobot->NumBodies++;
19234 jnt = body_0->JointInternal;
19235 cartesian_trajectory_planner_B.b_kstr_g = bname->size[0] * bname->size[1];
19236 bname->size[0] = 1;
19237 bname->size[1] = jnt->Type->size[1];
19238 cartes_emxEnsureCapacity_char_T(bname,
19239 cartesian_trajectory_planner_B.b_kstr_g);
19240 cartesian_trajectory_planner_B.loop_ub_j = jnt->Type->size[0] * jnt->
19241 Type->size[1] - 1;
19242 for (cartesian_trajectory_planner_B.b_kstr_g = 0;
19243 cartesian_trajectory_planner_B.b_kstr_g <=
19244 cartesian_trajectory_planner_B.loop_ub_j;
19245 cartesian_trajectory_planner_B.b_kstr_g++) {
19246 bname->data[cartesian_trajectory_planner_B.b_kstr_g] = jnt->Type->
19247 data[cartesian_trajectory_planner_B.b_kstr_g];
19248 }
19249
19250 for (cartesian_trajectory_planner_B.b_kstr_g = 0;
19251 cartesian_trajectory_planner_B.b_kstr_g < 5;
19252 cartesian_trajectory_planner_B.b_kstr_g++) {
19253 cartesian_trajectory_planner_B.b_js[cartesian_trajectory_planner_B.b_kstr_g]
19254 = tmp[cartesian_trajectory_planner_B.b_kstr_g];
19255 }
19256
19257 cartesian_trajectory_planner_B.b_bool_p = false;
19258 if (bname->size[1] == 5) {
19259 cartesian_trajectory_planner_B.b_kstr_g = 1;
19260 do {
19261 exitg2 = 0;
19262 if (cartesian_trajectory_planner_B.b_kstr_g - 1 < 5) {
19263 cartesian_trajectory_planner_B.loop_ub_j =
19264 cartesian_trajectory_planner_B.b_kstr_g - 1;
19265 if (bname->data[cartesian_trajectory_planner_B.loop_ub_j] !=
19266 cartesian_trajectory_planner_B.b_js[cartesian_trajectory_planner_B.loop_ub_j])
19267 {
19268 exitg2 = 1;
19269 } else {
19270 cartesian_trajectory_planner_B.b_kstr_g++;
19271 }
19272 } else {
19273 cartesian_trajectory_planner_B.b_bool_p = true;
19274 exitg2 = 1;
19275 }
19276 } while (exitg2 == 0);
19277 }
19278
19279 if (!cartesian_trajectory_planner_B.b_bool_p) {
19280 newrobot->NumNonFixedBodies++;
19281 jnt = body_0->JointInternal;
19282 cartesian_trajectory_planner_B.b_kstr_g = static_cast<int32_T>
19283 (body_0->Index) - 1;
19284 newrobot->PositionDoFMap[cartesian_trajectory_planner_B.b_kstr_g] =
19285 newrobot->PositionNumber + 1.0;
19286 newrobot->PositionDoFMap[cartesian_trajectory_planner_B.b_kstr_g + 8] =
19287 newrobot->PositionNumber + jnt->PositionNumber;
19288 jnt = body_0->JointInternal;
19289 cartesian_trajectory_planner_B.b_kstr_g = static_cast<int32_T>
19290 (body_0->Index) - 1;
19291 newrobot->VelocityDoFMap[cartesian_trajectory_planner_B.b_kstr_g] =
19292 newrobot->VelocityNumber + 1.0;
19293 newrobot->VelocityDoFMap[cartesian_trajectory_planner_B.b_kstr_g + 8] =
19294 newrobot->VelocityNumber + jnt->VelocityNumber;
19295 } else {
19296 cartesian_trajectory_planner_B.b_kstr_g = static_cast<int32_T>
19297 (body_0->Index);
19298 newrobot->PositionDoFMap[cartesian_trajectory_planner_B.b_kstr_g - 1] =
19299 0.0;
19300 newrobot->PositionDoFMap[cartesian_trajectory_planner_B.b_kstr_g + 7] =
19301 -1.0;
19302 cartesian_trajectory_planner_B.b_kstr_g = static_cast<int32_T>
19303 (body_0->Index);
19304 newrobot->VelocityDoFMap[cartesian_trajectory_planner_B.b_kstr_g - 1] =
19305 0.0;
19306 newrobot->VelocityDoFMap[cartesian_trajectory_planner_B.b_kstr_g + 7] =
19307 -1.0;
19308 }
19309
19310 jnt = body_0->JointInternal;
19311 newrobot->PositionNumber += jnt->PositionNumber;
19312 jnt = body_0->JointInternal;
19313 newrobot->VelocityNumber += jnt->VelocityNumber;
19314 }
19315
19316 cartesian_trajec_emxFree_char_T(&bname);
19317 cartesian_trajec_emxFree_char_T(&b_basename);
19318 obj->RigidBodyTreeInternal = newrobot;
19319}
19320
19321static h_robotics_core_internal_Damp_T *DampedBFGSwGradientProjection_D
19322 (h_robotics_core_internal_Damp_T *obj)
19323{
19324 h_robotics_core_internal_Damp_T *b_obj;
19325 int32_T i;
19326 static const char_T tmp[22] = { 'B', 'F', 'G', 'S', 'G', 'r', 'a', 'd', 'i',
19327 'e', 'n', 't', 'P', 'r', 'o', 'j', 'e', 'c', 't', 'i', 'o', 'n' };
19328
19329 b_obj = obj;
19330 obj->MaxNumIteration = 1500.0;
19331 obj->MaxTime = 10.0;
19332 obj->GradientTolerance = 1.0E-7;
19333 obj->SolutionTolerance = 1.0E-6;
19334 obj->ArmijoRuleBeta = 0.4;
19335 obj->ArmijoRuleSigma = 1.0E-5;
19336 obj->ConstraintsOn = true;
19337 obj->RandomRestart = true;
19338 obj->StepTolerance = 1.0E-14;
19339 for (i = 0; i < 22; i++) {
19340 obj->Name[i] = tmp[i];
19341 }
19342
19343 obj->ConstraintMatrix->size[0] = 0;
19344 obj->ConstraintMatrix->size[1] = 0;
19345 obj->ConstraintBound->size[0] = 0;
19346 obj->TimeObj.StartTime = -1.0;
19347 obj->TimeObjInternal.StartTime = -1.0;
19348 return b_obj;
19349}
19350
19351static void emxInitStruct_c_rigidBodyJoint1(c_rigidBodyJoint_cartesian_tr_T
19352 *pStruct)
19353{
19354 cartesian_trajec_emxInit_char_T(&pStruct->Type, 2);
19355 cartesian_trajec_emxInit_real_T(&pStruct->MotionSubspace, 2);
19356}
19357
19358static void emxInitStruct_o_robotics_manip_(o_robotics_manip_internal_Rig_T
19359 *pStruct)
19360{
19361 cartesian_trajec_emxInit_char_T(&pStruct->NameInternal, 2);
19362 emxInitStruct_c_rigidBodyJoint1(&pStruct->JointInternal);
19363}
19364
19365static void emxInitStruct_p_robotics_manip_(p_robotics_manip_internal_Rig_T
19366 *pStruct)
19367{
19368 emxInitStruct_o_robotics_manip_(&pStruct->Base);
19369}
19370
19371static void emxInitStruct_robotics_slmani_a(robotics_slmanip_internal_blo_T
19372 *pStruct)
19373{
19374 emxInitStruct_p_robotics_manip_(&pStruct->TreeInternal);
19375}
19376
19377static void emxInitStruct_n_robotics_manip_(n_robotics_manip_internal_Rig_T
19378 *pStruct)
19379{
19380 cartesian_trajec_emxInit_char_T(&pStruct->NameInternal, 2);
19381 emxInitStruct_c_rigidBodyJoint1(&pStruct->JointInternal);
19382}
19383
19384static n_robotics_manip_internal_Rig_T *cartesian_t_RigidBody_RigidBody
19385 (n_robotics_manip_internal_Rig_T *obj)
19386{
19387 n_robotics_manip_internal_Rig_T *b_obj;
19388 int8_T msubspace_data[36];
19389 emxArray_char_T_cartesian_tra_T *switch_expression;
19390 boolean_T b_bool;
19391 int32_T b_kstr;
19392 char_T b[8];
19393 char_T b_0[9];
19394 int32_T loop_ub;
19395 int8_T tmp[6];
19396 static const char_T tmp_0[13] = { 'e', 'd', 'o', '_', 'b', 'a', 's', 'e', '_',
19397 'l', 'i', 'n', 'k' };
19398
19399 static const char_T tmp_1[5] = { 'f', 'i', 'x', 'e', 'd' };
19400
19401 static const char_T tmp_2[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
19402
19403 static const char_T tmp_3[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
19404
19405 static const real_T tmp_4[16] = { 1.0, 0.0, -0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
19406 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
19407
19408 static const real_T tmp_5[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
19409 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
19410
19411 int32_T exitg1;
19412 b_obj = obj;
19413 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
19414 obj->NameInternal->size[0] = 1;
19415 obj->NameInternal->size[1] = 13;
19416 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);
19417 for (b_kstr = 0; b_kstr < 13; b_kstr++) {
19418 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
19419 }
19420
19421 obj->ParentIndex = 0.0;
19422 b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];
19423 obj->JointInternal.Type->size[0] = 1;
19424 obj->JointInternal.Type->size[1] = 5;
19425 cartes_emxEnsureCapacity_char_T(obj->JointInternal.Type, b_kstr);
19426 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
19427 obj->JointInternal.Type->data[b_kstr] = tmp_1[b_kstr];
19428 }
19429
19430 cartesian_trajec_emxInit_char_T(&switch_expression, 2);
19431 b_kstr = switch_expression->size[0] * switch_expression->size[1];
19432 switch_expression->size[0] = 1;
19433 switch_expression->size[1] = obj->JointInternal.Type->size[1];
19434 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
19435 loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]
19436 - 1;
19437 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
19438 switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];
19439 }
19440
19441 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
19442 b[b_kstr] = tmp_2[b_kstr];
19443 }
19444
19445 b_bool = false;
19446 if (switch_expression->size[1] == 8) {
19447 b_kstr = 1;
19448 do {
19449 exitg1 = 0;
19450 if (b_kstr - 1 < 8) {
19451 loop_ub = b_kstr - 1;
19452 if (switch_expression->data[loop_ub] != b[loop_ub]) {
19453 exitg1 = 1;
19454 } else {
19455 b_kstr++;
19456 }
19457 } else {
19458 b_bool = true;
19459 exitg1 = 1;
19460 }
19461 } while (exitg1 == 0);
19462 }
19463
19464 if (b_bool) {
19465 b_kstr = 0;
19466 } else {
19467 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
19468 b_0[b_kstr] = tmp_3[b_kstr];
19469 }
19470
19471 b_bool = false;
19472 if (switch_expression->size[1] == 9) {
19473 b_kstr = 1;
19474 do {
19475 exitg1 = 0;
19476 if (b_kstr - 1 < 9) {
19477 loop_ub = b_kstr - 1;
19478 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
19479 exitg1 = 1;
19480 } else {
19481 b_kstr++;
19482 }
19483 } else {
19484 b_bool = true;
19485 exitg1 = 1;
19486 }
19487 } while (exitg1 == 0);
19488 }
19489
19490 if (b_bool) {
19491 b_kstr = 1;
19492 } else {
19493 b_kstr = -1;
19494 }
19495 }
19496
19497 cartesian_trajec_emxFree_char_T(&switch_expression);
19498 switch (b_kstr) {
19499 case 0:
19500 tmp[0] = 0;
19501 tmp[1] = 0;
19502 tmp[2] = 1;
19503 tmp[3] = 0;
19504 tmp[4] = 0;
19505 tmp[5] = 0;
19506 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
19507 msubspace_data[b_kstr] = tmp[b_kstr];
19508 }
19509
19510 obj->JointInternal.PositionNumber = 1.0;
19511 obj->JointInternal.JointAxisInternal[0] = 0.0;
19512 obj->JointInternal.JointAxisInternal[1] = 0.0;
19513 obj->JointInternal.JointAxisInternal[2] = 1.0;
19514 break;
19515
19516 case 1:
19517 tmp[0] = 0;
19518 tmp[1] = 0;
19519 tmp[2] = 0;
19520 tmp[3] = 0;
19521 tmp[4] = 0;
19522 tmp[5] = 1;
19523 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
19524 msubspace_data[b_kstr] = tmp[b_kstr];
19525 }
19526
19527 obj->JointInternal.PositionNumber = 1.0;
19528 obj->JointInternal.JointAxisInternal[0] = 0.0;
19529 obj->JointInternal.JointAxisInternal[1] = 0.0;
19530 obj->JointInternal.JointAxisInternal[2] = 1.0;
19531 break;
19532
19533 default:
19534 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
19535 msubspace_data[b_kstr] = 0;
19536 }
19537
19538 obj->JointInternal.PositionNumber = 0.0;
19539 obj->JointInternal.JointAxisInternal[0] = 0.0;
19540 obj->JointInternal.JointAxisInternal[1] = 0.0;
19541 obj->JointInternal.JointAxisInternal[2] = 0.0;
19542 break;
19543 }
19544
19545 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
19546 obj->JointInternal.MotionSubspace->size[1];
19547 obj->JointInternal.MotionSubspace->size[0] = 6;
19548 obj->JointInternal.MotionSubspace->size[1] = 1;
19549 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
19550 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
19551 obj->JointInternal.MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
19552 }
19553
19554 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
19555 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_4[b_kstr];
19556 }
19557
19558 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
19559 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_5[b_kstr];
19560 }
19561
19562 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
19563 obj->JointInternal.MotionSubspace->size[1];
19564 obj->JointInternal.MotionSubspace->size[0] = 6;
19565 obj->JointInternal.MotionSubspace->size[1] = 1;
19566 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
19567 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
19568 obj->JointInternal.MotionSubspace->data[b_kstr] = 0.0;
19569 }
19570
19571 obj->JointInternal.JointAxisInternal[0] = 0.0;
19572 obj->JointInternal.JointAxisInternal[1] = 0.0;
19573 obj->JointInternal.JointAxisInternal[2] = 0.0;
19574 return b_obj;
19575}
19576
19577static n_robotics_manip_internal_Rig_T *cartesian_RigidBody_RigidBody_a
19578 (n_robotics_manip_internal_Rig_T *obj)
19579{
19580 n_robotics_manip_internal_Rig_T *b_obj;
19581 int8_T msubspace_data[36];
19582 emxArray_char_T_cartesian_tra_T *switch_expression;
19583 boolean_T b_bool;
19584 int32_T b_kstr;
19585 char_T b[8];
19586 char_T b_0[9];
19587 int32_T loop_ub;
19588 int8_T tmp[6];
19589 static const char_T tmp_0[10] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
19590 '1' };
19591
19592 static const char_T tmp_1[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
19593
19594 static const char_T tmp_2[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
19595
19596 static const real_T tmp_3[16] = { 1.0, 0.0, -0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
19597 0.0, 1.0, 0.0, 0.0, 0.0, 0.337, 1.0 };
19598
19599 static const real_T tmp_4[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
19600 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
19601
19602 static const real_T tmp_5[36] = { 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
19603 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
19604 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
19605
19606 int32_T exitg1;
19607 b_obj = obj;
19608 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
19609 obj->NameInternal->size[0] = 1;
19610 obj->NameInternal->size[1] = 10;
19611 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);
19612 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
19613 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
19614 }
19615
19616 obj->ParentIndex = 1.0;
19617 b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];
19618 obj->JointInternal.Type->size[0] = 1;
19619 obj->JointInternal.Type->size[1] = 8;
19620 cartes_emxEnsureCapacity_char_T(obj->JointInternal.Type, b_kstr);
19621 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
19622 obj->JointInternal.Type->data[b_kstr] = tmp_1[b_kstr];
19623 }
19624
19625 cartesian_trajec_emxInit_char_T(&switch_expression, 2);
19626 b_kstr = switch_expression->size[0] * switch_expression->size[1];
19627 switch_expression->size[0] = 1;
19628 switch_expression->size[1] = obj->JointInternal.Type->size[1];
19629 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
19630 loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]
19631 - 1;
19632 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
19633 switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];
19634 }
19635
19636 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
19637 b[b_kstr] = tmp_1[b_kstr];
19638 }
19639
19640 b_bool = false;
19641 if (switch_expression->size[1] == 8) {
19642 b_kstr = 1;
19643 do {
19644 exitg1 = 0;
19645 if (b_kstr - 1 < 8) {
19646 loop_ub = b_kstr - 1;
19647 if (switch_expression->data[loop_ub] != b[loop_ub]) {
19648 exitg1 = 1;
19649 } else {
19650 b_kstr++;
19651 }
19652 } else {
19653 b_bool = true;
19654 exitg1 = 1;
19655 }
19656 } while (exitg1 == 0);
19657 }
19658
19659 if (b_bool) {
19660 b_kstr = 0;
19661 } else {
19662 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
19663 b_0[b_kstr] = tmp_2[b_kstr];
19664 }
19665
19666 b_bool = false;
19667 if (switch_expression->size[1] == 9) {
19668 b_kstr = 1;
19669 do {
19670 exitg1 = 0;
19671 if (b_kstr - 1 < 9) {
19672 loop_ub = b_kstr - 1;
19673 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
19674 exitg1 = 1;
19675 } else {
19676 b_kstr++;
19677 }
19678 } else {
19679 b_bool = true;
19680 exitg1 = 1;
19681 }
19682 } while (exitg1 == 0);
19683 }
19684
19685 if (b_bool) {
19686 b_kstr = 1;
19687 } else {
19688 b_kstr = -1;
19689 }
19690 }
19691
19692 cartesian_trajec_emxFree_char_T(&switch_expression);
19693 switch (b_kstr) {
19694 case 0:
19695 tmp[0] = 0;
19696 tmp[1] = 0;
19697 tmp[2] = 1;
19698 tmp[3] = 0;
19699 tmp[4] = 0;
19700 tmp[5] = 0;
19701 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
19702 msubspace_data[b_kstr] = tmp[b_kstr];
19703 }
19704
19705 obj->JointInternal.PositionNumber = 1.0;
19706 obj->JointInternal.JointAxisInternal[0] = 0.0;
19707 obj->JointInternal.JointAxisInternal[1] = 0.0;
19708 obj->JointInternal.JointAxisInternal[2] = 1.0;
19709 break;
19710
19711 case 1:
19712 tmp[0] = 0;
19713 tmp[1] = 0;
19714 tmp[2] = 0;
19715 tmp[3] = 0;
19716 tmp[4] = 0;
19717 tmp[5] = 1;
19718 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
19719 msubspace_data[b_kstr] = tmp[b_kstr];
19720 }
19721
19722 obj->JointInternal.PositionNumber = 1.0;
19723 obj->JointInternal.JointAxisInternal[0] = 0.0;
19724 obj->JointInternal.JointAxisInternal[1] = 0.0;
19725 obj->JointInternal.JointAxisInternal[2] = 1.0;
19726 break;
19727
19728 default:
19729 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
19730 msubspace_data[b_kstr] = 0;
19731 }
19732
19733 obj->JointInternal.PositionNumber = 0.0;
19734 obj->JointInternal.JointAxisInternal[0] = 0.0;
19735 obj->JointInternal.JointAxisInternal[1] = 0.0;
19736 obj->JointInternal.JointAxisInternal[2] = 0.0;
19737 break;
19738 }
19739
19740 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
19741 obj->JointInternal.MotionSubspace->size[1];
19742 obj->JointInternal.MotionSubspace->size[0] = 6;
19743 obj->JointInternal.MotionSubspace->size[1] = 1;
19744 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
19745 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
19746 obj->JointInternal.MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
19747 }
19748
19749 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
19750 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_3[b_kstr];
19751 }
19752
19753 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
19754 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_4[b_kstr];
19755 }
19756
19757 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
19758 obj->JointInternal.MotionSubspace->size[1];
19759 obj->JointInternal.MotionSubspace->size[0] = 6;
19760 obj->JointInternal.MotionSubspace->size[1] = 1;
19761 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
19762 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
19763 obj->JointInternal.MotionSubspace->data[b_kstr] = tmp_5[b_kstr];
19764 }
19765
19766 obj->JointInternal.JointAxisInternal[0] = 0.0;
19767 obj->JointInternal.JointAxisInternal[1] = 0.0;
19768 obj->JointInternal.JointAxisInternal[2] = 1.0;
19769 return b_obj;
19770}
19771
19772static n_robotics_manip_internal_Rig_T *cartesia_RigidBody_RigidBody_as
19773 (n_robotics_manip_internal_Rig_T *obj)
19774{
19775 n_robotics_manip_internal_Rig_T *b_obj;
19776 int8_T msubspace_data[36];
19777 emxArray_char_T_cartesian_tra_T *switch_expression;
19778 boolean_T b_bool;
19779 int32_T b_kstr;
19780 char_T b[8];
19781 char_T b_0[9];
19782 int32_T loop_ub;
19783 int8_T tmp[6];
19784 static const char_T tmp_0[10] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
19785 '2' };
19786
19787 static const char_T tmp_1[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
19788
19789 static const char_T tmp_2[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
19790
19791 static const real_T tmp_3[16] = { 1.0, 0.0, -0.0, 0.0, 0.0,
19792 4.8965888601467475E-12, 1.0, 0.0, 0.0, -1.0, 4.8965888601467475E-12, 0.0,
19793 0.0, 0.0, 0.0, 1.0 };
19794
19795 static const real_T tmp_4[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
19796 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
19797
19798 static const real_T tmp_5[36] = { 0.0, 0.0, -1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
19799 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
19800 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
19801
19802 int32_T exitg1;
19803 b_obj = obj;
19804 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
19805 obj->NameInternal->size[0] = 1;
19806 obj->NameInternal->size[1] = 10;
19807 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);
19808 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
19809 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
19810 }
19811
19812 obj->ParentIndex = 2.0;
19813 b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];
19814 obj->JointInternal.Type->size[0] = 1;
19815 obj->JointInternal.Type->size[1] = 8;
19816 cartes_emxEnsureCapacity_char_T(obj->JointInternal.Type, b_kstr);
19817 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
19818 obj->JointInternal.Type->data[b_kstr] = tmp_1[b_kstr];
19819 }
19820
19821 cartesian_trajec_emxInit_char_T(&switch_expression, 2);
19822 b_kstr = switch_expression->size[0] * switch_expression->size[1];
19823 switch_expression->size[0] = 1;
19824 switch_expression->size[1] = obj->JointInternal.Type->size[1];
19825 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
19826 loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]
19827 - 1;
19828 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
19829 switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];
19830 }
19831
19832 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
19833 b[b_kstr] = tmp_1[b_kstr];
19834 }
19835
19836 b_bool = false;
19837 if (switch_expression->size[1] == 8) {
19838 b_kstr = 1;
19839 do {
19840 exitg1 = 0;
19841 if (b_kstr - 1 < 8) {
19842 loop_ub = b_kstr - 1;
19843 if (switch_expression->data[loop_ub] != b[loop_ub]) {
19844 exitg1 = 1;
19845 } else {
19846 b_kstr++;
19847 }
19848 } else {
19849 b_bool = true;
19850 exitg1 = 1;
19851 }
19852 } while (exitg1 == 0);
19853 }
19854
19855 if (b_bool) {
19856 b_kstr = 0;
19857 } else {
19858 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
19859 b_0[b_kstr] = tmp_2[b_kstr];
19860 }
19861
19862 b_bool = false;
19863 if (switch_expression->size[1] == 9) {
19864 b_kstr = 1;
19865 do {
19866 exitg1 = 0;
19867 if (b_kstr - 1 < 9) {
19868 loop_ub = b_kstr - 1;
19869 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
19870 exitg1 = 1;
19871 } else {
19872 b_kstr++;
19873 }
19874 } else {
19875 b_bool = true;
19876 exitg1 = 1;
19877 }
19878 } while (exitg1 == 0);
19879 }
19880
19881 if (b_bool) {
19882 b_kstr = 1;
19883 } else {
19884 b_kstr = -1;
19885 }
19886 }
19887
19888 cartesian_trajec_emxFree_char_T(&switch_expression);
19889 switch (b_kstr) {
19890 case 0:
19891 tmp[0] = 0;
19892 tmp[1] = 0;
19893 tmp[2] = 1;
19894 tmp[3] = 0;
19895 tmp[4] = 0;
19896 tmp[5] = 0;
19897 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
19898 msubspace_data[b_kstr] = tmp[b_kstr];
19899 }
19900
19901 obj->JointInternal.PositionNumber = 1.0;
19902 obj->JointInternal.JointAxisInternal[0] = 0.0;
19903 obj->JointInternal.JointAxisInternal[1] = 0.0;
19904 obj->JointInternal.JointAxisInternal[2] = 1.0;
19905 break;
19906
19907 case 1:
19908 tmp[0] = 0;
19909 tmp[1] = 0;
19910 tmp[2] = 0;
19911 tmp[3] = 0;
19912 tmp[4] = 0;
19913 tmp[5] = 1;
19914 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
19915 msubspace_data[b_kstr] = tmp[b_kstr];
19916 }
19917
19918 obj->JointInternal.PositionNumber = 1.0;
19919 obj->JointInternal.JointAxisInternal[0] = 0.0;
19920 obj->JointInternal.JointAxisInternal[1] = 0.0;
19921 obj->JointInternal.JointAxisInternal[2] = 1.0;
19922 break;
19923
19924 default:
19925 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
19926 msubspace_data[b_kstr] = 0;
19927 }
19928
19929 obj->JointInternal.PositionNumber = 0.0;
19930 obj->JointInternal.JointAxisInternal[0] = 0.0;
19931 obj->JointInternal.JointAxisInternal[1] = 0.0;
19932 obj->JointInternal.JointAxisInternal[2] = 0.0;
19933 break;
19934 }
19935
19936 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
19937 obj->JointInternal.MotionSubspace->size[1];
19938 obj->JointInternal.MotionSubspace->size[0] = 6;
19939 obj->JointInternal.MotionSubspace->size[1] = 1;
19940 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
19941 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
19942 obj->JointInternal.MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
19943 }
19944
19945 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
19946 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_3[b_kstr];
19947 }
19948
19949 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
19950 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_4[b_kstr];
19951 }
19952
19953 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
19954 obj->JointInternal.MotionSubspace->size[1];
19955 obj->JointInternal.MotionSubspace->size[0] = 6;
19956 obj->JointInternal.MotionSubspace->size[1] = 1;
19957 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
19958 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
19959 obj->JointInternal.MotionSubspace->data[b_kstr] = tmp_5[b_kstr];
19960 }
19961
19962 obj->JointInternal.JointAxisInternal[0] = 0.0;
19963 obj->JointInternal.JointAxisInternal[1] = 0.0;
19964 obj->JointInternal.JointAxisInternal[2] = -1.0;
19965 return b_obj;
19966}
19967
19968static n_robotics_manip_internal_Rig_T *cartesi_RigidBody_RigidBody_ast
19969 (n_robotics_manip_internal_Rig_T *obj)
19970{
19971 n_robotics_manip_internal_Rig_T *b_obj;
19972 int8_T msubspace_data[36];
19973 emxArray_char_T_cartesian_tra_T *switch_expression;
19974 boolean_T b_bool;
19975 int32_T b_kstr;
19976 char_T b[8];
19977 char_T b_0[9];
19978 int32_T loop_ub;
19979 int8_T tmp[6];
19980 static const char_T tmp_0[10] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
19981 '3' };
19982
19983 static const char_T tmp_1[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
19984
19985 static const char_T tmp_2[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
19986
19987 static const real_T tmp_3[16] = { 1.0, 2.0682310711021444E-13,
19988 2.0682310711021444E-13, 0.0, 2.0682310711021444E-13, -1.0, -0.0, 0.0,
19989 2.0682310711021444E-13, 4.2775797634723234E-26, -1.0, 0.0, 0.0, 0.2105, 0.0,
19990 1.0 };
19991
19992 static const real_T tmp_4[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
19993 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
19994
19995 static const real_T tmp_5[36] = { 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
19996 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
19997 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
19998
19999 int32_T exitg1;
20000 b_obj = obj;
20001 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
20002 obj->NameInternal->size[0] = 1;
20003 obj->NameInternal->size[1] = 10;
20004 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);
20005 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
20006 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
20007 }
20008
20009 obj->ParentIndex = 3.0;
20010 b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];
20011 obj->JointInternal.Type->size[0] = 1;
20012 obj->JointInternal.Type->size[1] = 8;
20013 cartes_emxEnsureCapacity_char_T(obj->JointInternal.Type, b_kstr);
20014 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
20015 obj->JointInternal.Type->data[b_kstr] = tmp_1[b_kstr];
20016 }
20017
20018 cartesian_trajec_emxInit_char_T(&switch_expression, 2);
20019 b_kstr = switch_expression->size[0] * switch_expression->size[1];
20020 switch_expression->size[0] = 1;
20021 switch_expression->size[1] = obj->JointInternal.Type->size[1];
20022 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
20023 loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]
20024 - 1;
20025 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
20026 switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];
20027 }
20028
20029 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
20030 b[b_kstr] = tmp_1[b_kstr];
20031 }
20032
20033 b_bool = false;
20034 if (switch_expression->size[1] == 8) {
20035 b_kstr = 1;
20036 do {
20037 exitg1 = 0;
20038 if (b_kstr - 1 < 8) {
20039 loop_ub = b_kstr - 1;
20040 if (switch_expression->data[loop_ub] != b[loop_ub]) {
20041 exitg1 = 1;
20042 } else {
20043 b_kstr++;
20044 }
20045 } else {
20046 b_bool = true;
20047 exitg1 = 1;
20048 }
20049 } while (exitg1 == 0);
20050 }
20051
20052 if (b_bool) {
20053 b_kstr = 0;
20054 } else {
20055 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
20056 b_0[b_kstr] = tmp_2[b_kstr];
20057 }
20058
20059 b_bool = false;
20060 if (switch_expression->size[1] == 9) {
20061 b_kstr = 1;
20062 do {
20063 exitg1 = 0;
20064 if (b_kstr - 1 < 9) {
20065 loop_ub = b_kstr - 1;
20066 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
20067 exitg1 = 1;
20068 } else {
20069 b_kstr++;
20070 }
20071 } else {
20072 b_bool = true;
20073 exitg1 = 1;
20074 }
20075 } while (exitg1 == 0);
20076 }
20077
20078 if (b_bool) {
20079 b_kstr = 1;
20080 } else {
20081 b_kstr = -1;
20082 }
20083 }
20084
20085 cartesian_trajec_emxFree_char_T(&switch_expression);
20086 switch (b_kstr) {
20087 case 0:
20088 tmp[0] = 0;
20089 tmp[1] = 0;
20090 tmp[2] = 1;
20091 tmp[3] = 0;
20092 tmp[4] = 0;
20093 tmp[5] = 0;
20094 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
20095 msubspace_data[b_kstr] = tmp[b_kstr];
20096 }
20097
20098 obj->JointInternal.PositionNumber = 1.0;
20099 obj->JointInternal.JointAxisInternal[0] = 0.0;
20100 obj->JointInternal.JointAxisInternal[1] = 0.0;
20101 obj->JointInternal.JointAxisInternal[2] = 1.0;
20102 break;
20103
20104 case 1:
20105 tmp[0] = 0;
20106 tmp[1] = 0;
20107 tmp[2] = 0;
20108 tmp[3] = 0;
20109 tmp[4] = 0;
20110 tmp[5] = 1;
20111 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
20112 msubspace_data[b_kstr] = tmp[b_kstr];
20113 }
20114
20115 obj->JointInternal.PositionNumber = 1.0;
20116 obj->JointInternal.JointAxisInternal[0] = 0.0;
20117 obj->JointInternal.JointAxisInternal[1] = 0.0;
20118 obj->JointInternal.JointAxisInternal[2] = 1.0;
20119 break;
20120
20121 default:
20122 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
20123 msubspace_data[b_kstr] = 0;
20124 }
20125
20126 obj->JointInternal.PositionNumber = 0.0;
20127 obj->JointInternal.JointAxisInternal[0] = 0.0;
20128 obj->JointInternal.JointAxisInternal[1] = 0.0;
20129 obj->JointInternal.JointAxisInternal[2] = 0.0;
20130 break;
20131 }
20132
20133 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
20134 obj->JointInternal.MotionSubspace->size[1];
20135 obj->JointInternal.MotionSubspace->size[0] = 6;
20136 obj->JointInternal.MotionSubspace->size[1] = 1;
20137 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
20138 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
20139 obj->JointInternal.MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
20140 }
20141
20142 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
20143 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_3[b_kstr];
20144 }
20145
20146 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
20147 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_4[b_kstr];
20148 }
20149
20150 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
20151 obj->JointInternal.MotionSubspace->size[1];
20152 obj->JointInternal.MotionSubspace->size[0] = 6;
20153 obj->JointInternal.MotionSubspace->size[1] = 1;
20154 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
20155 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
20156 obj->JointInternal.MotionSubspace->data[b_kstr] = tmp_5[b_kstr];
20157 }
20158
20159 obj->JointInternal.JointAxisInternal[0] = 0.0;
20160 obj->JointInternal.JointAxisInternal[1] = 0.0;
20161 obj->JointInternal.JointAxisInternal[2] = 1.0;
20162 return b_obj;
20163}
20164
20165static n_robotics_manip_internal_Rig_T *cartes_RigidBody_RigidBody_astw
20166 (n_robotics_manip_internal_Rig_T *obj)
20167{
20168 n_robotics_manip_internal_Rig_T *b_obj;
20169 int8_T msubspace_data[36];
20170 emxArray_char_T_cartesian_tra_T *switch_expression;
20171 boolean_T b_bool;
20172 int32_T b_kstr;
20173 char_T b[8];
20174 char_T b_0[9];
20175 int32_T loop_ub;
20176 int8_T tmp[6];
20177 static const char_T tmp_0[10] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
20178 '4' };
20179
20180 static const char_T tmp_1[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
20181
20182 static const char_T tmp_2[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
20183
20184 static const real_T tmp_3[16] = { 1.0, 0.0, -0.0, 0.0, 0.0,
20185 4.8965888601467475E-12, 1.0, 0.0, 0.0, -1.0, 4.8965888601467475E-12, 0.0,
20186 0.0, -0.268, 0.0, 1.0 };
20187
20188 static const real_T tmp_4[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
20189 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
20190
20191 static const real_T tmp_5[36] = { 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
20192 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
20193 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
20194
20195 int32_T exitg1;
20196 b_obj = obj;
20197 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
20198 obj->NameInternal->size[0] = 1;
20199 obj->NameInternal->size[1] = 10;
20200 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);
20201 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
20202 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
20203 }
20204
20205 obj->ParentIndex = 4.0;
20206 b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];
20207 obj->JointInternal.Type->size[0] = 1;
20208 obj->JointInternal.Type->size[1] = 8;
20209 cartes_emxEnsureCapacity_char_T(obj->JointInternal.Type, b_kstr);
20210 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
20211 obj->JointInternal.Type->data[b_kstr] = tmp_1[b_kstr];
20212 }
20213
20214 cartesian_trajec_emxInit_char_T(&switch_expression, 2);
20215 b_kstr = switch_expression->size[0] * switch_expression->size[1];
20216 switch_expression->size[0] = 1;
20217 switch_expression->size[1] = obj->JointInternal.Type->size[1];
20218 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
20219 loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]
20220 - 1;
20221 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
20222 switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];
20223 }
20224
20225 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
20226 b[b_kstr] = tmp_1[b_kstr];
20227 }
20228
20229 b_bool = false;
20230 if (switch_expression->size[1] == 8) {
20231 b_kstr = 1;
20232 do {
20233 exitg1 = 0;
20234 if (b_kstr - 1 < 8) {
20235 loop_ub = b_kstr - 1;
20236 if (switch_expression->data[loop_ub] != b[loop_ub]) {
20237 exitg1 = 1;
20238 } else {
20239 b_kstr++;
20240 }
20241 } else {
20242 b_bool = true;
20243 exitg1 = 1;
20244 }
20245 } while (exitg1 == 0);
20246 }
20247
20248 if (b_bool) {
20249 b_kstr = 0;
20250 } else {
20251 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
20252 b_0[b_kstr] = tmp_2[b_kstr];
20253 }
20254
20255 b_bool = false;
20256 if (switch_expression->size[1] == 9) {
20257 b_kstr = 1;
20258 do {
20259 exitg1 = 0;
20260 if (b_kstr - 1 < 9) {
20261 loop_ub = b_kstr - 1;
20262 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
20263 exitg1 = 1;
20264 } else {
20265 b_kstr++;
20266 }
20267 } else {
20268 b_bool = true;
20269 exitg1 = 1;
20270 }
20271 } while (exitg1 == 0);
20272 }
20273
20274 if (b_bool) {
20275 b_kstr = 1;
20276 } else {
20277 b_kstr = -1;
20278 }
20279 }
20280
20281 cartesian_trajec_emxFree_char_T(&switch_expression);
20282 switch (b_kstr) {
20283 case 0:
20284 tmp[0] = 0;
20285 tmp[1] = 0;
20286 tmp[2] = 1;
20287 tmp[3] = 0;
20288 tmp[4] = 0;
20289 tmp[5] = 0;
20290 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
20291 msubspace_data[b_kstr] = tmp[b_kstr];
20292 }
20293
20294 obj->JointInternal.PositionNumber = 1.0;
20295 obj->JointInternal.JointAxisInternal[0] = 0.0;
20296 obj->JointInternal.JointAxisInternal[1] = 0.0;
20297 obj->JointInternal.JointAxisInternal[2] = 1.0;
20298 break;
20299
20300 case 1:
20301 tmp[0] = 0;
20302 tmp[1] = 0;
20303 tmp[2] = 0;
20304 tmp[3] = 0;
20305 tmp[4] = 0;
20306 tmp[5] = 1;
20307 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
20308 msubspace_data[b_kstr] = tmp[b_kstr];
20309 }
20310
20311 obj->JointInternal.PositionNumber = 1.0;
20312 obj->JointInternal.JointAxisInternal[0] = 0.0;
20313 obj->JointInternal.JointAxisInternal[1] = 0.0;
20314 obj->JointInternal.JointAxisInternal[2] = 1.0;
20315 break;
20316
20317 default:
20318 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
20319 msubspace_data[b_kstr] = 0;
20320 }
20321
20322 obj->JointInternal.PositionNumber = 0.0;
20323 obj->JointInternal.JointAxisInternal[0] = 0.0;
20324 obj->JointInternal.JointAxisInternal[1] = 0.0;
20325 obj->JointInternal.JointAxisInternal[2] = 0.0;
20326 break;
20327 }
20328
20329 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
20330 obj->JointInternal.MotionSubspace->size[1];
20331 obj->JointInternal.MotionSubspace->size[0] = 6;
20332 obj->JointInternal.MotionSubspace->size[1] = 1;
20333 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
20334 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
20335 obj->JointInternal.MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
20336 }
20337
20338 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
20339 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_3[b_kstr];
20340 }
20341
20342 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
20343 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_4[b_kstr];
20344 }
20345
20346 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
20347 obj->JointInternal.MotionSubspace->size[1];
20348 obj->JointInternal.MotionSubspace->size[0] = 6;
20349 obj->JointInternal.MotionSubspace->size[1] = 1;
20350 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
20351 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
20352 obj->JointInternal.MotionSubspace->data[b_kstr] = tmp_5[b_kstr];
20353 }
20354
20355 obj->JointInternal.JointAxisInternal[0] = 0.0;
20356 obj->JointInternal.JointAxisInternal[1] = 0.0;
20357 obj->JointInternal.JointAxisInternal[2] = 1.0;
20358 return b_obj;
20359}
20360
20361static n_robotics_manip_internal_Rig_T *carte_RigidBody_RigidBody_astwh
20362 (n_robotics_manip_internal_Rig_T *obj)
20363{
20364 n_robotics_manip_internal_Rig_T *b_obj;
20365 int8_T msubspace_data[36];
20366 emxArray_char_T_cartesian_tra_T *switch_expression;
20367 boolean_T b_bool;
20368 int32_T b_kstr;
20369 char_T b[8];
20370 char_T b_0[9];
20371 int32_T loop_ub;
20372 int8_T tmp[6];
20373 static const char_T tmp_0[10] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
20374 '5' };
20375
20376 static const char_T tmp_1[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
20377
20378 static const char_T tmp_2[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
20379
20380 static const real_T tmp_3[16] = { 1.0, 0.0, -0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
20381 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
20382
20383 static const real_T tmp_4[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
20384 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
20385
20386 static const real_T tmp_5[36] = { 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
20387 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
20388 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
20389
20390 int32_T exitg1;
20391 b_obj = obj;
20392 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
20393 obj->NameInternal->size[0] = 1;
20394 obj->NameInternal->size[1] = 10;
20395 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);
20396 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
20397 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
20398 }
20399
20400 obj->ParentIndex = 5.0;
20401 b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];
20402 obj->JointInternal.Type->size[0] = 1;
20403 obj->JointInternal.Type->size[1] = 8;
20404 cartes_emxEnsureCapacity_char_T(obj->JointInternal.Type, b_kstr);
20405 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
20406 obj->JointInternal.Type->data[b_kstr] = tmp_1[b_kstr];
20407 }
20408
20409 cartesian_trajec_emxInit_char_T(&switch_expression, 2);
20410 b_kstr = switch_expression->size[0] * switch_expression->size[1];
20411 switch_expression->size[0] = 1;
20412 switch_expression->size[1] = obj->JointInternal.Type->size[1];
20413 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
20414 loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]
20415 - 1;
20416 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
20417 switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];
20418 }
20419
20420 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
20421 b[b_kstr] = tmp_1[b_kstr];
20422 }
20423
20424 b_bool = false;
20425 if (switch_expression->size[1] == 8) {
20426 b_kstr = 1;
20427 do {
20428 exitg1 = 0;
20429 if (b_kstr - 1 < 8) {
20430 loop_ub = b_kstr - 1;
20431 if (switch_expression->data[loop_ub] != b[loop_ub]) {
20432 exitg1 = 1;
20433 } else {
20434 b_kstr++;
20435 }
20436 } else {
20437 b_bool = true;
20438 exitg1 = 1;
20439 }
20440 } while (exitg1 == 0);
20441 }
20442
20443 if (b_bool) {
20444 b_kstr = 0;
20445 } else {
20446 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
20447 b_0[b_kstr] = tmp_2[b_kstr];
20448 }
20449
20450 b_bool = false;
20451 if (switch_expression->size[1] == 9) {
20452 b_kstr = 1;
20453 do {
20454 exitg1 = 0;
20455 if (b_kstr - 1 < 9) {
20456 loop_ub = b_kstr - 1;
20457 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
20458 exitg1 = 1;
20459 } else {
20460 b_kstr++;
20461 }
20462 } else {
20463 b_bool = true;
20464 exitg1 = 1;
20465 }
20466 } while (exitg1 == 0);
20467 }
20468
20469 if (b_bool) {
20470 b_kstr = 1;
20471 } else {
20472 b_kstr = -1;
20473 }
20474 }
20475
20476 cartesian_trajec_emxFree_char_T(&switch_expression);
20477 switch (b_kstr) {
20478 case 0:
20479 tmp[0] = 0;
20480 tmp[1] = 0;
20481 tmp[2] = 1;
20482 tmp[3] = 0;
20483 tmp[4] = 0;
20484 tmp[5] = 0;
20485 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
20486 msubspace_data[b_kstr] = tmp[b_kstr];
20487 }
20488
20489 obj->JointInternal.PositionNumber = 1.0;
20490 obj->JointInternal.JointAxisInternal[0] = 0.0;
20491 obj->JointInternal.JointAxisInternal[1] = 0.0;
20492 obj->JointInternal.JointAxisInternal[2] = 1.0;
20493 break;
20494
20495 case 1:
20496 tmp[0] = 0;
20497 tmp[1] = 0;
20498 tmp[2] = 0;
20499 tmp[3] = 0;
20500 tmp[4] = 0;
20501 tmp[5] = 1;
20502 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
20503 msubspace_data[b_kstr] = tmp[b_kstr];
20504 }
20505
20506 obj->JointInternal.PositionNumber = 1.0;
20507 obj->JointInternal.JointAxisInternal[0] = 0.0;
20508 obj->JointInternal.JointAxisInternal[1] = 0.0;
20509 obj->JointInternal.JointAxisInternal[2] = 1.0;
20510 break;
20511
20512 default:
20513 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
20514 msubspace_data[b_kstr] = 0;
20515 }
20516
20517 obj->JointInternal.PositionNumber = 0.0;
20518 obj->JointInternal.JointAxisInternal[0] = 0.0;
20519 obj->JointInternal.JointAxisInternal[1] = 0.0;
20520 obj->JointInternal.JointAxisInternal[2] = 0.0;
20521 break;
20522 }
20523
20524 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
20525 obj->JointInternal.MotionSubspace->size[1];
20526 obj->JointInternal.MotionSubspace->size[0] = 6;
20527 obj->JointInternal.MotionSubspace->size[1] = 1;
20528 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
20529 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
20530 obj->JointInternal.MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
20531 }
20532
20533 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
20534 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_3[b_kstr];
20535 }
20536
20537 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
20538 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_4[b_kstr];
20539 }
20540
20541 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
20542 obj->JointInternal.MotionSubspace->size[1];
20543 obj->JointInternal.MotionSubspace->size[0] = 6;
20544 obj->JointInternal.MotionSubspace->size[1] = 1;
20545 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
20546 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
20547 obj->JointInternal.MotionSubspace->data[b_kstr] = tmp_5[b_kstr];
20548 }
20549
20550 obj->JointInternal.JointAxisInternal[0] = 0.0;
20551 obj->JointInternal.JointAxisInternal[1] = 1.0;
20552 obj->JointInternal.JointAxisInternal[2] = 0.0;
20553 return b_obj;
20554}
20555
20556static n_robotics_manip_internal_Rig_T *cart_RigidBody_RigidBody_astwhq
20557 (n_robotics_manip_internal_Rig_T *obj)
20558{
20559 n_robotics_manip_internal_Rig_T *b_obj;
20560 int8_T msubspace_data[36];
20561 emxArray_char_T_cartesian_tra_T *switch_expression;
20562 boolean_T b_bool;
20563 int32_T b_kstr;
20564 char_T b[8];
20565 char_T b_0[9];
20566 int32_T loop_ub;
20567 int8_T tmp[6];
20568 static const char_T tmp_0[10] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
20569 '6' };
20570
20571 static const char_T tmp_1[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
20572
20573 static const char_T tmp_2[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
20574
20575 static const real_T tmp_3[16] = { 1.0, 0.0, -0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
20576 0.0, 1.0, 0.0, 0.0, 0.0, 0.1745, 1.0 };
20577
20578 static const real_T tmp_4[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
20579 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
20580
20581 static const real_T tmp_5[36] = { 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
20582 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
20583 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
20584
20585 int32_T exitg1;
20586 b_obj = obj;
20587 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
20588 obj->NameInternal->size[0] = 1;
20589 obj->NameInternal->size[1] = 10;
20590 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);
20591 for (b_kstr = 0; b_kstr < 10; b_kstr++) {
20592 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
20593 }
20594
20595 obj->ParentIndex = 6.0;
20596 b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];
20597 obj->JointInternal.Type->size[0] = 1;
20598 obj->JointInternal.Type->size[1] = 8;
20599 cartes_emxEnsureCapacity_char_T(obj->JointInternal.Type, b_kstr);
20600 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
20601 obj->JointInternal.Type->data[b_kstr] = tmp_1[b_kstr];
20602 }
20603
20604 cartesian_trajec_emxInit_char_T(&switch_expression, 2);
20605 b_kstr = switch_expression->size[0] * switch_expression->size[1];
20606 switch_expression->size[0] = 1;
20607 switch_expression->size[1] = obj->JointInternal.Type->size[1];
20608 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
20609 loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]
20610 - 1;
20611 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
20612 switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];
20613 }
20614
20615 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
20616 b[b_kstr] = tmp_1[b_kstr];
20617 }
20618
20619 b_bool = false;
20620 if (switch_expression->size[1] == 8) {
20621 b_kstr = 1;
20622 do {
20623 exitg1 = 0;
20624 if (b_kstr - 1 < 8) {
20625 loop_ub = b_kstr - 1;
20626 if (switch_expression->data[loop_ub] != b[loop_ub]) {
20627 exitg1 = 1;
20628 } else {
20629 b_kstr++;
20630 }
20631 } else {
20632 b_bool = true;
20633 exitg1 = 1;
20634 }
20635 } while (exitg1 == 0);
20636 }
20637
20638 if (b_bool) {
20639 b_kstr = 0;
20640 } else {
20641 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
20642 b_0[b_kstr] = tmp_2[b_kstr];
20643 }
20644
20645 b_bool = false;
20646 if (switch_expression->size[1] == 9) {
20647 b_kstr = 1;
20648 do {
20649 exitg1 = 0;
20650 if (b_kstr - 1 < 9) {
20651 loop_ub = b_kstr - 1;
20652 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
20653 exitg1 = 1;
20654 } else {
20655 b_kstr++;
20656 }
20657 } else {
20658 b_bool = true;
20659 exitg1 = 1;
20660 }
20661 } while (exitg1 == 0);
20662 }
20663
20664 if (b_bool) {
20665 b_kstr = 1;
20666 } else {
20667 b_kstr = -1;
20668 }
20669 }
20670
20671 cartesian_trajec_emxFree_char_T(&switch_expression);
20672 switch (b_kstr) {
20673 case 0:
20674 tmp[0] = 0;
20675 tmp[1] = 0;
20676 tmp[2] = 1;
20677 tmp[3] = 0;
20678 tmp[4] = 0;
20679 tmp[5] = 0;
20680 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
20681 msubspace_data[b_kstr] = tmp[b_kstr];
20682 }
20683
20684 obj->JointInternal.PositionNumber = 1.0;
20685 obj->JointInternal.JointAxisInternal[0] = 0.0;
20686 obj->JointInternal.JointAxisInternal[1] = 0.0;
20687 obj->JointInternal.JointAxisInternal[2] = 1.0;
20688 break;
20689
20690 case 1:
20691 tmp[0] = 0;
20692 tmp[1] = 0;
20693 tmp[2] = 0;
20694 tmp[3] = 0;
20695 tmp[4] = 0;
20696 tmp[5] = 1;
20697 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
20698 msubspace_data[b_kstr] = tmp[b_kstr];
20699 }
20700
20701 obj->JointInternal.PositionNumber = 1.0;
20702 obj->JointInternal.JointAxisInternal[0] = 0.0;
20703 obj->JointInternal.JointAxisInternal[1] = 0.0;
20704 obj->JointInternal.JointAxisInternal[2] = 1.0;
20705 break;
20706
20707 default:
20708 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
20709 msubspace_data[b_kstr] = 0;
20710 }
20711
20712 obj->JointInternal.PositionNumber = 0.0;
20713 obj->JointInternal.JointAxisInternal[0] = 0.0;
20714 obj->JointInternal.JointAxisInternal[1] = 0.0;
20715 obj->JointInternal.JointAxisInternal[2] = 0.0;
20716 break;
20717 }
20718
20719 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
20720 obj->JointInternal.MotionSubspace->size[1];
20721 obj->JointInternal.MotionSubspace->size[0] = 6;
20722 obj->JointInternal.MotionSubspace->size[1] = 1;
20723 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
20724 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
20725 obj->JointInternal.MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
20726 }
20727
20728 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
20729 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_3[b_kstr];
20730 }
20731
20732 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
20733 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_4[b_kstr];
20734 }
20735
20736 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
20737 obj->JointInternal.MotionSubspace->size[1];
20738 obj->JointInternal.MotionSubspace->size[0] = 6;
20739 obj->JointInternal.MotionSubspace->size[1] = 1;
20740 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
20741 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
20742 obj->JointInternal.MotionSubspace->data[b_kstr] = tmp_5[b_kstr];
20743 }
20744
20745 obj->JointInternal.JointAxisInternal[0] = 0.0;
20746 obj->JointInternal.JointAxisInternal[1] = 0.0;
20747 obj->JointInternal.JointAxisInternal[2] = 1.0;
20748 return b_obj;
20749}
20750
20751static n_robotics_manip_internal_Rig_T *car_RigidBody_RigidBody_astwhqf
20752 (n_robotics_manip_internal_Rig_T *obj)
20753{
20754 n_robotics_manip_internal_Rig_T *b_obj;
20755 int8_T msubspace_data[36];
20756 emxArray_char_T_cartesian_tra_T *switch_expression;
20757 boolean_T b_bool;
20758 int32_T b_kstr;
20759 char_T b[8];
20760 char_T b_0[9];
20761 int32_T loop_ub;
20762 int8_T tmp[6];
20763 static const char_T tmp_0[11] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',
20764 'e', 'e' };
20765
20766 static const char_T tmp_1[5] = { 'f', 'i', 'x', 'e', 'd' };
20767
20768 static const char_T tmp_2[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
20769
20770 static const char_T tmp_3[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
20771
20772 static const real_T tmp_4[16] = { 1.0, 0.0, -0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
20773 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
20774
20775 static const real_T tmp_5[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
20776 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
20777
20778 int32_T exitg1;
20779 b_obj = obj;
20780 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
20781 obj->NameInternal->size[0] = 1;
20782 obj->NameInternal->size[1] = 11;
20783 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);
20784 for (b_kstr = 0; b_kstr < 11; b_kstr++) {
20785 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
20786 }
20787
20788 obj->ParentIndex = 7.0;
20789 b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];
20790 obj->JointInternal.Type->size[0] = 1;
20791 obj->JointInternal.Type->size[1] = 5;
20792 cartes_emxEnsureCapacity_char_T(obj->JointInternal.Type, b_kstr);
20793 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
20794 obj->JointInternal.Type->data[b_kstr] = tmp_1[b_kstr];
20795 }
20796
20797 cartesian_trajec_emxInit_char_T(&switch_expression, 2);
20798 b_kstr = switch_expression->size[0] * switch_expression->size[1];
20799 switch_expression->size[0] = 1;
20800 switch_expression->size[1] = obj->JointInternal.Type->size[1];
20801 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
20802 loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]
20803 - 1;
20804 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
20805 switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];
20806 }
20807
20808 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
20809 b[b_kstr] = tmp_2[b_kstr];
20810 }
20811
20812 b_bool = false;
20813 if (switch_expression->size[1] == 8) {
20814 b_kstr = 1;
20815 do {
20816 exitg1 = 0;
20817 if (b_kstr - 1 < 8) {
20818 loop_ub = b_kstr - 1;
20819 if (switch_expression->data[loop_ub] != b[loop_ub]) {
20820 exitg1 = 1;
20821 } else {
20822 b_kstr++;
20823 }
20824 } else {
20825 b_bool = true;
20826 exitg1 = 1;
20827 }
20828 } while (exitg1 == 0);
20829 }
20830
20831 if (b_bool) {
20832 b_kstr = 0;
20833 } else {
20834 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
20835 b_0[b_kstr] = tmp_3[b_kstr];
20836 }
20837
20838 b_bool = false;
20839 if (switch_expression->size[1] == 9) {
20840 b_kstr = 1;
20841 do {
20842 exitg1 = 0;
20843 if (b_kstr - 1 < 9) {
20844 loop_ub = b_kstr - 1;
20845 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
20846 exitg1 = 1;
20847 } else {
20848 b_kstr++;
20849 }
20850 } else {
20851 b_bool = true;
20852 exitg1 = 1;
20853 }
20854 } while (exitg1 == 0);
20855 }
20856
20857 if (b_bool) {
20858 b_kstr = 1;
20859 } else {
20860 b_kstr = -1;
20861 }
20862 }
20863
20864 cartesian_trajec_emxFree_char_T(&switch_expression);
20865 switch (b_kstr) {
20866 case 0:
20867 tmp[0] = 0;
20868 tmp[1] = 0;
20869 tmp[2] = 1;
20870 tmp[3] = 0;
20871 tmp[4] = 0;
20872 tmp[5] = 0;
20873 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
20874 msubspace_data[b_kstr] = tmp[b_kstr];
20875 }
20876
20877 obj->JointInternal.PositionNumber = 1.0;
20878 obj->JointInternal.JointAxisInternal[0] = 0.0;
20879 obj->JointInternal.JointAxisInternal[1] = 0.0;
20880 obj->JointInternal.JointAxisInternal[2] = 1.0;
20881 break;
20882
20883 case 1:
20884 tmp[0] = 0;
20885 tmp[1] = 0;
20886 tmp[2] = 0;
20887 tmp[3] = 0;
20888 tmp[4] = 0;
20889 tmp[5] = 1;
20890 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
20891 msubspace_data[b_kstr] = tmp[b_kstr];
20892 }
20893
20894 obj->JointInternal.PositionNumber = 1.0;
20895 obj->JointInternal.JointAxisInternal[0] = 0.0;
20896 obj->JointInternal.JointAxisInternal[1] = 0.0;
20897 obj->JointInternal.JointAxisInternal[2] = 1.0;
20898 break;
20899
20900 default:
20901 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
20902 msubspace_data[b_kstr] = 0;
20903 }
20904
20905 obj->JointInternal.PositionNumber = 0.0;
20906 obj->JointInternal.JointAxisInternal[0] = 0.0;
20907 obj->JointInternal.JointAxisInternal[1] = 0.0;
20908 obj->JointInternal.JointAxisInternal[2] = 0.0;
20909 break;
20910 }
20911
20912 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
20913 obj->JointInternal.MotionSubspace->size[1];
20914 obj->JointInternal.MotionSubspace->size[0] = 6;
20915 obj->JointInternal.MotionSubspace->size[1] = 1;
20916 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
20917 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
20918 obj->JointInternal.MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
20919 }
20920
20921 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
20922 obj->JointInternal.JointToParentTransform[b_kstr] = tmp_4[b_kstr];
20923 }
20924
20925 for (b_kstr = 0; b_kstr < 16; b_kstr++) {
20926 obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_5[b_kstr];
20927 }
20928
20929 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
20930 obj->JointInternal.MotionSubspace->size[1];
20931 obj->JointInternal.MotionSubspace->size[0] = 6;
20932 obj->JointInternal.MotionSubspace->size[1] = 1;
20933 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
20934 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
20935 obj->JointInternal.MotionSubspace->data[b_kstr] = 0.0;
20936 }
20937
20938 obj->JointInternal.JointAxisInternal[0] = 0.0;
20939 obj->JointInternal.JointAxisInternal[1] = 0.0;
20940 obj->JointInternal.JointAxisInternal[2] = 0.0;
20941 return b_obj;
20942}
20943
20944static o_robotics_manip_internal_Rig_T *ca_RigidBody_RigidBody_astwhqf2
20945 (o_robotics_manip_internal_Rig_T *obj)
20946{
20947 o_robotics_manip_internal_Rig_T *b_obj;
20948 int8_T msubspace_data[36];
20949 emxArray_char_T_cartesian_tra_T *switch_expression;
20950 boolean_T b_bool;
20951 int32_T b_kstr;
20952 char_T b[8];
20953 char_T b_0[9];
20954 int32_T loop_ub;
20955 int8_T tmp[6];
20956 static const char_T tmp_0[5] = { 'w', 'o', 'r', 'l', 'd' };
20957
20958 static const char_T tmp_1[5] = { 'f', 'i', 'x', 'e', 'd' };
20959
20960 static const char_T tmp_2[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };
20961
20962 static const char_T tmp_3[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };
20963
20964 int32_T exitg1;
20965 b_obj = obj;
20966 b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];
20967 obj->NameInternal->size[0] = 1;
20968 obj->NameInternal->size[1] = 5;
20969 cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);
20970 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
20971 obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];
20972 }
20973
20974 b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];
20975 obj->JointInternal.Type->size[0] = 1;
20976 obj->JointInternal.Type->size[1] = 5;
20977 cartes_emxEnsureCapacity_char_T(obj->JointInternal.Type, b_kstr);
20978 for (b_kstr = 0; b_kstr < 5; b_kstr++) {
20979 obj->JointInternal.Type->data[b_kstr] = tmp_1[b_kstr];
20980 }
20981
20982 cartesian_trajec_emxInit_char_T(&switch_expression, 2);
20983 b_kstr = switch_expression->size[0] * switch_expression->size[1];
20984 switch_expression->size[0] = 1;
20985 switch_expression->size[1] = obj->JointInternal.Type->size[1];
20986 cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);
20987 loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]
20988 - 1;
20989 for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {
20990 switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];
20991 }
20992
20993 for (b_kstr = 0; b_kstr < 8; b_kstr++) {
20994 b[b_kstr] = tmp_2[b_kstr];
20995 }
20996
20997 b_bool = false;
20998 if (switch_expression->size[1] == 8) {
20999 b_kstr = 1;
21000 do {
21001 exitg1 = 0;
21002 if (b_kstr - 1 < 8) {
21003 loop_ub = b_kstr - 1;
21004 if (switch_expression->data[loop_ub] != b[loop_ub]) {
21005 exitg1 = 1;
21006 } else {
21007 b_kstr++;
21008 }
21009 } else {
21010 b_bool = true;
21011 exitg1 = 1;
21012 }
21013 } while (exitg1 == 0);
21014 }
21015
21016 if (b_bool) {
21017 b_kstr = 0;
21018 } else {
21019 for (b_kstr = 0; b_kstr < 9; b_kstr++) {
21020 b_0[b_kstr] = tmp_3[b_kstr];
21021 }
21022
21023 b_bool = false;
21024 if (switch_expression->size[1] == 9) {
21025 b_kstr = 1;
21026 do {
21027 exitg1 = 0;
21028 if (b_kstr - 1 < 9) {
21029 loop_ub = b_kstr - 1;
21030 if (switch_expression->data[loop_ub] != b_0[loop_ub]) {
21031 exitg1 = 1;
21032 } else {
21033 b_kstr++;
21034 }
21035 } else {
21036 b_bool = true;
21037 exitg1 = 1;
21038 }
21039 } while (exitg1 == 0);
21040 }
21041
21042 if (b_bool) {
21043 b_kstr = 1;
21044 } else {
21045 b_kstr = -1;
21046 }
21047 }
21048
21049 cartesian_trajec_emxFree_char_T(&switch_expression);
21050 switch (b_kstr) {
21051 case 0:
21052 tmp[0] = 0;
21053 tmp[1] = 0;
21054 tmp[2] = 1;
21055 tmp[3] = 0;
21056 tmp[4] = 0;
21057 tmp[5] = 0;
21058 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
21059 msubspace_data[b_kstr] = tmp[b_kstr];
21060 }
21061
21062 obj->JointInternal.PositionNumber = 1.0;
21063 obj->JointInternal.JointAxisInternal[0] = 0.0;
21064 obj->JointInternal.JointAxisInternal[1] = 0.0;
21065 obj->JointInternal.JointAxisInternal[2] = 1.0;
21066 break;
21067
21068 case 1:
21069 tmp[0] = 0;
21070 tmp[1] = 0;
21071 tmp[2] = 0;
21072 tmp[3] = 0;
21073 tmp[4] = 0;
21074 tmp[5] = 1;
21075 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
21076 msubspace_data[b_kstr] = tmp[b_kstr];
21077 }
21078
21079 obj->JointInternal.PositionNumber = 1.0;
21080 obj->JointInternal.JointAxisInternal[0] = 0.0;
21081 obj->JointInternal.JointAxisInternal[1] = 0.0;
21082 obj->JointInternal.JointAxisInternal[2] = 1.0;
21083 break;
21084
21085 default:
21086 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
21087 msubspace_data[b_kstr] = 0;
21088 }
21089
21090 obj->JointInternal.PositionNumber = 0.0;
21091 obj->JointInternal.JointAxisInternal[0] = 0.0;
21092 obj->JointInternal.JointAxisInternal[1] = 0.0;
21093 obj->JointInternal.JointAxisInternal[2] = 0.0;
21094 break;
21095 }
21096
21097 b_kstr = obj->JointInternal.MotionSubspace->size[0] *
21098 obj->JointInternal.MotionSubspace->size[1];
21099 obj->JointInternal.MotionSubspace->size[0] = 6;
21100 obj->JointInternal.MotionSubspace->size[1] = 1;
21101 cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);
21102 for (b_kstr = 0; b_kstr < 6; b_kstr++) {
21103 obj->JointInternal.MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];
21104 }
21105
21106 return b_obj;
21107}
21108
21109static p_robotics_manip_internal_Rig_T *car_RigidBodyTree_RigidBodyTree
21110 (p_robotics_manip_internal_Rig_T *obj, n_robotics_manip_internal_Rig_T *iobj_0,
21111 n_robotics_manip_internal_Rig_T *iobj_1, n_robotics_manip_internal_Rig_T
21112 *iobj_2, n_robotics_manip_internal_Rig_T *iobj_3,
21113 n_robotics_manip_internal_Rig_T *iobj_4, n_robotics_manip_internal_Rig_T
21114 *iobj_5, n_robotics_manip_internal_Rig_T *iobj_6,
21115 n_robotics_manip_internal_Rig_T *iobj_7)
21116{
21117 p_robotics_manip_internal_Rig_T *b_obj;
21118 int32_T i;
21119 static const int8_T tmp[16] = { 0, 1, 2, 3, 4, 5, 6, 0, -1, 1, 2, 3, 4, 5, 6,
21120 -1 };
21121
21122 b_obj = obj;
21123 obj->Bodies[0] = cartesian_t_RigidBody_RigidBody(iobj_0);
21124 obj->Bodies[0]->Index = 1.0;
21125 obj->Bodies[1] = cartesian_RigidBody_RigidBody_a(iobj_7);
21126 obj->Bodies[1]->Index = 2.0;
21127 obj->Bodies[2] = cartesia_RigidBody_RigidBody_as(iobj_1);
21128 obj->Bodies[2]->Index = 3.0;
21129 obj->Bodies[3] = cartesi_RigidBody_RigidBody_ast(iobj_2);
21130 obj->Bodies[3]->Index = 4.0;
21131 obj->Bodies[4] = cartes_RigidBody_RigidBody_astw(iobj_3);
21132 obj->Bodies[4]->Index = 5.0;
21133 obj->Bodies[5] = carte_RigidBody_RigidBody_astwh(iobj_4);
21134 obj->Bodies[5]->Index = 6.0;
21135 obj->Bodies[6] = cart_RigidBody_RigidBody_astwhq(iobj_5);
21136 obj->Bodies[6]->Index = 7.0;
21137 obj->Bodies[7] = car_RigidBody_RigidBody_astwhqf(iobj_6);
21138 obj->Bodies[7]->Index = 8.0;
21139 obj->NumBodies = 8.0;
21140 obj->PositionNumber = 6.0;
21141 obj->VelocityNumber = 6.0;
21142 for (i = 0; i < 16; i++) {
21143 obj->PositionDoFMap[i] = tmp[i];
21144 }
21145
21146 ca_RigidBody_RigidBody_astwhqf2(&obj->Base);
21147 return b_obj;
21148}
21149
21150// Model step function
21151void cartesian_trajectory_planner_step(void)
21152{
21153 robotics_slmanip_internal_b_a_T *obj;
21154 emxArray_real_T_cartesian_tra_T *b;
21155
21156 // Outputs for Atomic SubSystem: '<Root>/Subscribe'
21157 // MATLABSystem: '<S7>/SourceBlock' incorporates:
21158 // Inport: '<S10>/In1'
21159
21160 cartesian_trajectory_planner_B.b_varargout_1 =
21161 Sub_cartesian_trajectory_planner_269.getLatestMessage
21162 (&cartesian_trajectory_planner_B.b_varargout_2_m);
21163
21164 // Outputs for Enabled SubSystem: '<S7>/Enabled Subsystem' incorporates:
21165 // EnablePort: '<S10>/Enable'
21166
21167 if (cartesian_trajectory_planner_B.b_varargout_1) {
21168 cartesian_trajectory_planner_B.In1 =
21169 cartesian_trajectory_planner_B.b_varargout_2_m;
21170 }
21171
21172 // End of MATLABSystem: '<S7>/SourceBlock'
21173 // End of Outputs for SubSystem: '<S7>/Enabled Subsystem'
21174 // End of Outputs for SubSystem: '<Root>/Subscribe'
21175
21176 // MATLAB Function: '<Root>/MATLAB Function1'
21177 cartesian_trajectory_planner_B.time =
21178 cartesian_trajectory_planner_B.In1.Clock_.Nsec / 1.0E+9 +
21179 cartesian_trajectory_planner_B.In1.Clock_.Sec;
21180
21181 // SignalConversion generated from: '<Root>/Transform Trajectory' incorporates:
21182 // MATLABSystem: '<S8>/Get Parameter'
21183 // MATLABSystem: '<S8>/Get Parameter1'
21184
21185 ParamGet_cartesian_trajectory_planner_272.get_parameter
21186 (&cartesian_trajectory_planner_B.signal1[0]);
21187 ParamGet_cartesian_trajectory_planner_273.get_parameter
21188 (&cartesian_trajectory_planner_B.signal1[1]);
21189
21190 // MATLABSystem: '<S11>/Get Parameter3'
21191 ParamGet_cartesian_trajectory_planner_280.get_parameter
21192 (&cartesian_trajectory_planner_B.value_m);
21193
21194 // MATLABSystem: '<S11>/Get Parameter4'
21195 ParamGet_cartesian_trajectory_planner_281.get_parameter
21196 (&cartesian_trajectory_planner_B.value_p);
21197
21198 // MATLABSystem: '<S11>/Get Parameter5'
21199 ParamGet_cartesian_trajectory_planner_282.get_parameter
21200 (&cartesian_trajectory_planner_B.value_d);
21201
21202 // MATLABSystem: '<S11>/Get Parameter6'
21203 ParamGet_cartesian_trajectory_planner_283.get_parameter
21204 (&cartesian_trajectory_planner_B.value_g);
21205
21206 // MATLABSystem: '<S11>/Get Parameter'
21207 ParamGet_cartesian_trajectory_planner_277.get_parameter
21208 (&cartesian_trajectory_planner_B.value_c);
21209
21210 // MATLABSystem: '<S11>/Get Parameter1'
21211 ParamGet_cartesian_trajectory_planner_278.get_parameter
21212 (&cartesian_trajectory_planner_B.value_cx);
21213
21214 // MATLABSystem: '<S11>/Get Parameter2'
21215 ParamGet_cartesian_trajectory_planner_279.get_parameter
21216 (&cartesian_trajectory_planner_B.value_i);
21217
21218 // MATLABSystem: '<Root>/Coordinate Transformation Conversion' incorporates:
21219 // MATLABSystem: '<S11>/Get Parameter'
21220 // MATLABSystem: '<S11>/Get Parameter1'
21221 // MATLABSystem: '<S11>/Get Parameter2'
21222 // MATLABSystem: '<S11>/Get Parameter3'
21223 // MATLABSystem: '<S11>/Get Parameter4'
21224 // MATLABSystem: '<S11>/Get Parameter5'
21225 // MATLABSystem: '<S11>/Get Parameter6'
21226
21227 cartesian_trajectory_planner_B.b_dx = 1.0 / sqrt
21228 (((cartesian_trajectory_planner_B.value_m *
21229 cartesian_trajectory_planner_B.value_m +
21230 cartesian_trajectory_planner_B.value_p *
21231 cartesian_trajectory_planner_B.value_p) +
21232 cartesian_trajectory_planner_B.value_d *
21233 cartesian_trajectory_planner_B.value_d) +
21234 cartesian_trajectory_planner_B.value_g *
21235 cartesian_trajectory_planner_B.value_g);
21236 cartesian_trajectory_planner_B.value_m *= cartesian_trajectory_planner_B.b_dx;
21237 cartesian_trajectory_planner_B.value_p *= cartesian_trajectory_planner_B.b_dx;
21238 cartesian_trajectory_planner_B.value_d *= cartesian_trajectory_planner_B.b_dx;
21239 cartesian_trajectory_planner_B.value_g *= cartesian_trajectory_planner_B.b_dx;
21240 cartesian_trajectory_planner_B.b_dx = cartesian_trajectory_planner_B.value_g *
21241 cartesian_trajectory_planner_B.value_g;
21242 cartesian_trajectory_planner_B.tempR_tmp_d =
21243 cartesian_trajectory_planner_B.value_d *
21244 cartesian_trajectory_planner_B.value_d;
21245 cartesian_trajectory_planner_B.tempR[0] = 1.0 -
21246 (cartesian_trajectory_planner_B.tempR_tmp_d +
21247 cartesian_trajectory_planner_B.b_dx) * 2.0;
21248 cartesian_trajectory_planner_B.tempR_tmp =
21249 cartesian_trajectory_planner_B.value_p *
21250 cartesian_trajectory_planner_B.value_d;
21251 cartesian_trajectory_planner_B.tempR_tmp_g =
21252 cartesian_trajectory_planner_B.value_m *
21253 cartesian_trajectory_planner_B.value_g;
21254 cartesian_trajectory_planner_B.tempR[1] =
21255 (cartesian_trajectory_planner_B.tempR_tmp -
21256 cartesian_trajectory_planner_B.tempR_tmp_g) * 2.0;
21257 cartesian_trajectory_planner_B.tempR_tmp_l =
21258 cartesian_trajectory_planner_B.value_p *
21259 cartesian_trajectory_planner_B.value_g;
21260 cartesian_trajectory_planner_B.tempR_tmp_f =
21261 cartesian_trajectory_planner_B.value_m *
21262 cartesian_trajectory_planner_B.value_d;
21263 cartesian_trajectory_planner_B.tempR[2] =
21264 (cartesian_trajectory_planner_B.tempR_tmp_l +
21265 cartesian_trajectory_planner_B.tempR_tmp_f) * 2.0;
21266 cartesian_trajectory_planner_B.tempR[3] =
21267 (cartesian_trajectory_planner_B.tempR_tmp +
21268 cartesian_trajectory_planner_B.tempR_tmp_g) * 2.0;
21269 cartesian_trajectory_planner_B.tempR_tmp =
21270 cartesian_trajectory_planner_B.value_p *
21271 cartesian_trajectory_planner_B.value_p;
21272 cartesian_trajectory_planner_B.tempR[4] = 1.0 -
21273 (cartesian_trajectory_planner_B.tempR_tmp +
21274 cartesian_trajectory_planner_B.b_dx) * 2.0;
21275 cartesian_trajectory_planner_B.b_dx = cartesian_trajectory_planner_B.value_d *
21276 cartesian_trajectory_planner_B.value_g;
21277 cartesian_trajectory_planner_B.tempR_tmp_g =
21278 cartesian_trajectory_planner_B.value_m *
21279 cartesian_trajectory_planner_B.value_p;
21280 cartesian_trajectory_planner_B.tempR[5] = (cartesian_trajectory_planner_B.b_dx
21281 - cartesian_trajectory_planner_B.tempR_tmp_g) * 2.0;
21282 cartesian_trajectory_planner_B.tempR[6] =
21283 (cartesian_trajectory_planner_B.tempR_tmp_l -
21284 cartesian_trajectory_planner_B.tempR_tmp_f) * 2.0;
21285 cartesian_trajectory_planner_B.tempR[7] = (cartesian_trajectory_planner_B.b_dx
21286 + cartesian_trajectory_planner_B.tempR_tmp_g) * 2.0;
21287 cartesian_trajectory_planner_B.tempR[8] = 1.0 -
21288 (cartesian_trajectory_planner_B.tempR_tmp +
21289 cartesian_trajectory_planner_B.tempR_tmp_d) * 2.0;
21290 for (cartesian_trajectory_planner_B.b_k_e = 0;
21291 cartesian_trajectory_planner_B.b_k_e < 3;
21292 cartesian_trajectory_planner_B.b_k_e++) {
21293 cartesian_trajectory_planner_B.subsa_idx_1 =
21294 cartesian_trajectory_planner_B.b_k_e + 1;
21295 cartesian_trajectory_planner_B.R[cartesian_trajectory_planner_B.subsa_idx_1
21296 - 1] = cartesian_trajectory_planner_B.tempR
21297 [(cartesian_trajectory_planner_B.subsa_idx_1 - 1) * 3];
21298 cartesian_trajectory_planner_B.subsa_idx_1 =
21299 cartesian_trajectory_planner_B.b_k_e + 1;
21300 cartesian_trajectory_planner_B.R[cartesian_trajectory_planner_B.subsa_idx_1
21301 + 2] = cartesian_trajectory_planner_B.tempR
21302 [(cartesian_trajectory_planner_B.subsa_idx_1 - 1) * 3 + 1];
21303 cartesian_trajectory_planner_B.subsa_idx_1 =
21304 cartesian_trajectory_planner_B.b_k_e + 1;
21305 cartesian_trajectory_planner_B.R[cartesian_trajectory_planner_B.subsa_idx_1
21306 + 5] = cartesian_trajectory_planner_B.tempR
21307 [(cartesian_trajectory_planner_B.subsa_idx_1 - 1) * 3 + 2];
21308 }
21309
21310 memset(&cartesian_trajectory_planner_B.out[0], 0, sizeof(real_T) << 4U);
21311 for (cartesian_trajectory_planner_B.b_k_e = 0;
21312 cartesian_trajectory_planner_B.b_k_e < 3;
21313 cartesian_trajectory_planner_B.b_k_e++) {
21314 cartesian_trajectory_planner_B.subsa_idx_1 =
21315 cartesian_trajectory_planner_B.b_k_e << 2;
21316 cartesian_trajectory_planner_B.out[cartesian_trajectory_planner_B.subsa_idx_1]
21317 = cartesian_trajectory_planner_B.R[3 *
21318 cartesian_trajectory_planner_B.b_k_e];
21319 cartesian_trajectory_planner_B.out[cartesian_trajectory_planner_B.subsa_idx_1
21320 + 1] = cartesian_trajectory_planner_B.R[3 *
21321 cartesian_trajectory_planner_B.b_k_e + 1];
21322 cartesian_trajectory_planner_B.out[cartesian_trajectory_planner_B.subsa_idx_1
21323 + 2] = cartesian_trajectory_planner_B.R[3 *
21324 cartesian_trajectory_planner_B.b_k_e + 2];
21325 }
21326
21327 cartesian_trajectory_planner_B.out[15] = 1.0;
21328 cartesian_trajectory_planner_B.out[12] =
21329 cartesian_trajectory_planner_B.value_c;
21330 cartesian_trajectory_planner_B.out[13] =
21331 cartesian_trajectory_planner_B.value_cx;
21332 cartesian_trajectory_planner_B.out[14] =
21333 cartesian_trajectory_planner_B.value_i;
21334
21335 // MATLABSystem: '<S12>/Get Parameter3'
21336 ParamGet_cartesian_trajectory_planner_292.get_parameter
21337 (&cartesian_trajectory_planner_B.value_m);
21338
21339 // MATLABSystem: '<S12>/Get Parameter4'
21340 ParamGet_cartesian_trajectory_planner_293.get_parameter
21341 (&cartesian_trajectory_planner_B.value_p);
21342
21343 // MATLABSystem: '<S12>/Get Parameter5'
21344 ParamGet_cartesian_trajectory_planner_294.get_parameter
21345 (&cartesian_trajectory_planner_B.value_d);
21346
21347 // MATLABSystem: '<S12>/Get Parameter6'
21348 ParamGet_cartesian_trajectory_planner_295.get_parameter
21349 (&cartesian_trajectory_planner_B.value_g);
21350
21351 // MATLABSystem: '<S12>/Get Parameter'
21352 ParamGet_cartesian_trajectory_planner_289.get_parameter
21353 (&cartesian_trajectory_planner_B.value_c);
21354
21355 // MATLABSystem: '<S12>/Get Parameter1'
21356 ParamGet_cartesian_trajectory_planner_290.get_parameter
21357 (&cartesian_trajectory_planner_B.value_cx);
21358
21359 // MATLABSystem: '<S12>/Get Parameter2'
21360 ParamGet_cartesian_trajectory_planner_291.get_parameter
21361 (&cartesian_trajectory_planner_B.value_i);
21362
21363 // MATLABSystem: '<Root>/Coordinate Transformation Conversion1' incorporates:
21364 // MATLABSystem: '<S12>/Get Parameter'
21365 // MATLABSystem: '<S12>/Get Parameter1'
21366 // MATLABSystem: '<S12>/Get Parameter2'
21367 // MATLABSystem: '<S12>/Get Parameter3'
21368 // MATLABSystem: '<S12>/Get Parameter4'
21369 // MATLABSystem: '<S12>/Get Parameter5'
21370 // MATLABSystem: '<S12>/Get Parameter6'
21371
21372 cartesian_trajectory_planner_B.b_dx = 1.0 / sqrt
21373 (((cartesian_trajectory_planner_B.value_m *
21374 cartesian_trajectory_planner_B.value_m +
21375 cartesian_trajectory_planner_B.value_p *
21376 cartesian_trajectory_planner_B.value_p) +
21377 cartesian_trajectory_planner_B.value_d *
21378 cartesian_trajectory_planner_B.value_d) +
21379 cartesian_trajectory_planner_B.value_g *
21380 cartesian_trajectory_planner_B.value_g);
21381 cartesian_trajectory_planner_B.value_m *= cartesian_trajectory_planner_B.b_dx;
21382 cartesian_trajectory_planner_B.value_p *= cartesian_trajectory_planner_B.b_dx;
21383 cartesian_trajectory_planner_B.value_d *= cartesian_trajectory_planner_B.b_dx;
21384 cartesian_trajectory_planner_B.value_g *= cartesian_trajectory_planner_B.b_dx;
21385 cartesian_trajectory_planner_B.b_dx = cartesian_trajectory_planner_B.value_g *
21386 cartesian_trajectory_planner_B.value_g;
21387 cartesian_trajectory_planner_B.tempR_tmp_d =
21388 cartesian_trajectory_planner_B.value_d *
21389 cartesian_trajectory_planner_B.value_d;
21390 cartesian_trajectory_planner_B.tempR[0] = 1.0 -
21391 (cartesian_trajectory_planner_B.tempR_tmp_d +
21392 cartesian_trajectory_planner_B.b_dx) * 2.0;
21393 cartesian_trajectory_planner_B.tempR_tmp =
21394 cartesian_trajectory_planner_B.value_p *
21395 cartesian_trajectory_planner_B.value_d;
21396 cartesian_trajectory_planner_B.tempR_tmp_g =
21397 cartesian_trajectory_planner_B.value_m *
21398 cartesian_trajectory_planner_B.value_g;
21399 cartesian_trajectory_planner_B.tempR[1] =
21400 (cartesian_trajectory_planner_B.tempR_tmp -
21401 cartesian_trajectory_planner_B.tempR_tmp_g) * 2.0;
21402 cartesian_trajectory_planner_B.tempR_tmp_l =
21403 cartesian_trajectory_planner_B.value_p *
21404 cartesian_trajectory_planner_B.value_g;
21405 cartesian_trajectory_planner_B.tempR_tmp_f =
21406 cartesian_trajectory_planner_B.value_m *
21407 cartesian_trajectory_planner_B.value_d;
21408 cartesian_trajectory_planner_B.tempR[2] =
21409 (cartesian_trajectory_planner_B.tempR_tmp_l +
21410 cartesian_trajectory_planner_B.tempR_tmp_f) * 2.0;
21411 cartesian_trajectory_planner_B.tempR[3] =
21412 (cartesian_trajectory_planner_B.tempR_tmp +
21413 cartesian_trajectory_planner_B.tempR_tmp_g) * 2.0;
21414 cartesian_trajectory_planner_B.tempR_tmp =
21415 cartesian_trajectory_planner_B.value_p *
21416 cartesian_trajectory_planner_B.value_p;
21417 cartesian_trajectory_planner_B.tempR[4] = 1.0 -
21418 (cartesian_trajectory_planner_B.tempR_tmp +
21419 cartesian_trajectory_planner_B.b_dx) * 2.0;
21420 cartesian_trajectory_planner_B.b_dx = cartesian_trajectory_planner_B.value_d *
21421 cartesian_trajectory_planner_B.value_g;
21422 cartesian_trajectory_planner_B.tempR_tmp_g =
21423 cartesian_trajectory_planner_B.value_m *
21424 cartesian_trajectory_planner_B.value_p;
21425 cartesian_trajectory_planner_B.tempR[5] = (cartesian_trajectory_planner_B.b_dx
21426 - cartesian_trajectory_planner_B.tempR_tmp_g) * 2.0;
21427 cartesian_trajectory_planner_B.tempR[6] =
21428 (cartesian_trajectory_planner_B.tempR_tmp_l -
21429 cartesian_trajectory_planner_B.tempR_tmp_f) * 2.0;
21430 cartesian_trajectory_planner_B.tempR[7] = (cartesian_trajectory_planner_B.b_dx
21431 + cartesian_trajectory_planner_B.tempR_tmp_g) * 2.0;
21432 cartesian_trajectory_planner_B.tempR[8] = 1.0 -
21433 (cartesian_trajectory_planner_B.tempR_tmp +
21434 cartesian_trajectory_planner_B.tempR_tmp_d) * 2.0;
21435 for (cartesian_trajectory_planner_B.b_k_e = 0;
21436 cartesian_trajectory_planner_B.b_k_e < 3;
21437 cartesian_trajectory_planner_B.b_k_e++) {
21438 cartesian_trajectory_planner_B.subsa_idx_1 =
21439 cartesian_trajectory_planner_B.b_k_e + 1;
21440 cartesian_trajectory_planner_B.R[cartesian_trajectory_planner_B.subsa_idx_1
21441 - 1] = cartesian_trajectory_planner_B.tempR
21442 [(cartesian_trajectory_planner_B.subsa_idx_1 - 1) * 3];
21443 cartesian_trajectory_planner_B.subsa_idx_1 =
21444 cartesian_trajectory_planner_B.b_k_e + 1;
21445 cartesian_trajectory_planner_B.R[cartesian_trajectory_planner_B.subsa_idx_1
21446 + 2] = cartesian_trajectory_planner_B.tempR
21447 [(cartesian_trajectory_planner_B.subsa_idx_1 - 1) * 3 + 1];
21448 cartesian_trajectory_planner_B.subsa_idx_1 =
21449 cartesian_trajectory_planner_B.b_k_e + 1;
21450 cartesian_trajectory_planner_B.R[cartesian_trajectory_planner_B.subsa_idx_1
21451 + 5] = cartesian_trajectory_planner_B.tempR
21452 [(cartesian_trajectory_planner_B.subsa_idx_1 - 1) * 3 + 2];
21453 }
21454
21455 memset(&cartesian_trajectory_planner_B.out_l[0], 0, sizeof(real_T) << 4U);
21456 for (cartesian_trajectory_planner_B.b_k_e = 0;
21457 cartesian_trajectory_planner_B.b_k_e < 3;
21458 cartesian_trajectory_planner_B.b_k_e++) {
21459 cartesian_trajectory_planner_B.subsa_idx_1 =
21460 cartesian_trajectory_planner_B.b_k_e << 2;
21461 cartesian_trajectory_planner_B.out_l[cartesian_trajectory_planner_B.subsa_idx_1]
21462 = cartesian_trajectory_planner_B.R[3 *
21463 cartesian_trajectory_planner_B.b_k_e];
21464 cartesian_trajectory_planner_B.out_l[cartesian_trajectory_planner_B.subsa_idx_1
21465 + 1] = cartesian_trajectory_planner_B.R[3 *
21466 cartesian_trajectory_planner_B.b_k_e + 1];
21467 cartesian_trajectory_planner_B.out_l[cartesian_trajectory_planner_B.subsa_idx_1
21468 + 2] = cartesian_trajectory_planner_B.R[3 *
21469 cartesian_trajectory_planner_B.b_k_e + 2];
21470 }
21471
21472 cartesian_trajectory_planner_B.out_l[15] = 1.0;
21473 cartesian_trajectory_planner_B.out_l[12] =
21474 cartesian_trajectory_planner_B.value_c;
21475 cartesian_trajectory_planner_B.out_l[13] =
21476 cartesian_trajectory_planner_B.value_cx;
21477 cartesian_trajectory_planner_B.out_l[14] =
21478 cartesian_trajectory_planner_B.value_i;
21479
21480 // MATLABSystem: '<Root>/Transform Trajectory' incorporates:
21481 // MATLABSystem: '<Root>/Coordinate Transformation Conversion'
21482 // MATLABSystem: '<Root>/Coordinate Transformation Conversion1'
21483
21484 if (cartesian_trajectory_planner_DW.obj_e.TunablePropsChanged) {
21485 cartesian_trajectory_planner_DW.obj_e.TunablePropsChanged = false;
21486 }
21487
21488 cart_constructLinearTimeScaling(cartesian_trajectory_planner_B.signal1,
21489 cartesian_trajectory_planner_B.time, cartesian_trajectory_planner_B.b_f1);
21490 for (cartesian_trajectory_planner_B.b_k_e = 0;
21491 cartesian_trajectory_planner_B.b_k_e < 3;
21492 cartesian_trajectory_planner_B.b_k_e++) {
21493 // MATLABSystem: '<Root>/Coordinate Transformation Conversion' incorporates:
21494 // MATLABSystem: '<Root>/Coordinate Transformation Conversion1'
21495
21496 cartesian_trajectory_planner_B.subsa_idx_1 =
21497 cartesian_trajectory_planner_B.b_k_e << 2;
21498 cartesian_trajectory_planner_B.R[3 * cartesian_trajectory_planner_B.b_k_e] =
21499 cartesian_trajectory_planner_B.out[cartesian_trajectory_planner_B.subsa_idx_1];
21500 cartesian_trajectory_planner_B.out_lt[3 *
21501 cartesian_trajectory_planner_B.b_k_e] =
21502 cartesian_trajectory_planner_B.out_l[cartesian_trajectory_planner_B.subsa_idx_1];
21503
21504 // MATLABSystem: '<Root>/Coordinate Transformation Conversion' incorporates:
21505 // MATLABSystem: '<Root>/Coordinate Transformation Conversion1'
21506
21507 cartesian_trajectory_planner_B.out_tmp =
21508 cartesian_trajectory_planner_B.subsa_idx_1 + 1;
21509 cartesian_trajectory_planner_B.out_tmp_h = 3 *
21510 cartesian_trajectory_planner_B.b_k_e + 1;
21511 cartesian_trajectory_planner_B.R[cartesian_trajectory_planner_B.out_tmp_h] =
21512 cartesian_trajectory_planner_B.out[cartesian_trajectory_planner_B.out_tmp];
21513 cartesian_trajectory_planner_B.out_lt[cartesian_trajectory_planner_B.out_tmp_h]
21514 =
21515 cartesian_trajectory_planner_B.out_l[cartesian_trajectory_planner_B.out_tmp];
21516
21517 // MATLABSystem: '<Root>/Coordinate Transformation Conversion' incorporates:
21518 // MATLABSystem: '<Root>/Coordinate Transformation Conversion1'
21519
21520 cartesian_trajectory_planner_B.subsa_idx_1 += 2;
21521 cartesian_trajectory_planner_B.out_tmp = 3 *
21522 cartesian_trajectory_planner_B.b_k_e + 2;
21523 cartesian_trajectory_planner_B.R[cartesian_trajectory_planner_B.out_tmp] =
21524 cartesian_trajectory_planner_B.out[cartesian_trajectory_planner_B.subsa_idx_1];
21525 cartesian_trajectory_planner_B.out_lt[cartesian_trajectory_planner_B.out_tmp]
21526 =
21527 cartesian_trajectory_planner_B.out_l[cartesian_trajectory_planner_B.subsa_idx_1];
21528 }
21529
21530 cartesian_trajectory_pl_rottraj(cartesian_trajectory_planner_B.R,
21531 cartesian_trajectory_planner_B.out_lt, cartesian_trajectory_planner_B.b_f1,
21532 cartesian_trajectory_planner_B.tempR, cartesian_trajectory_planner_B.w,
21533 cartesian_trajectory_planner_B.a_p);
21534 memset(&cartesian_trajectory_planner_B.tfCalc[0], 0, sizeof(real_T) << 4U);
21535 cartesian_trajectory_planner_B.tfCalc[15] = 1.0;
21536 for (cartesian_trajectory_planner_B.b_k_e = 0;
21537 cartesian_trajectory_planner_B.b_k_e < 3;
21538 cartesian_trajectory_planner_B.b_k_e++) {
21539 // MATLABSystem: '<Root>/Coordinate Transformation Conversion'
21540 cartesian_trajectory_planner_B.time =
21541 cartesian_trajectory_planner_B.out[cartesian_trajectory_planner_B.b_k_e +
21542 12];
21543 cartesian_trajectory_planner_B.a_p[cartesian_trajectory_planner_B.b_k_e] =
21544 cartesian_trajectory_planner_B.time;
21545 cartesian_trajectory_planner_B.subsa_idx_1 =
21546 cartesian_trajectory_planner_B.b_k_e << 2;
21547 cartesian_trajectory_planner_B.tfCalc[cartesian_trajectory_planner_B.subsa_idx_1]
21548 = cartesian_trajectory_planner_B.tempR[3 *
21549 cartesian_trajectory_planner_B.b_k_e];
21550 cartesian_trajectory_planner_B.tfCalc[cartesian_trajectory_planner_B.subsa_idx_1
21551 + 1] = cartesian_trajectory_planner_B.tempR[3 *
21552 cartesian_trajectory_planner_B.b_k_e + 1];
21553 cartesian_trajectory_planner_B.tfCalc[cartesian_trajectory_planner_B.subsa_idx_1
21554 + 2] = cartesian_trajectory_planner_B.tempR[3 *
21555 cartesian_trajectory_planner_B.b_k_e + 2];
21556 cartesian_trajectory_planner_B.tfCalc_tmp[cartesian_trajectory_planner_B.b_k_e]
21557 =
21558 cartesian_trajectory_planner_B.out_l[cartesian_trajectory_planner_B.b_k_e
21559 + 12] - cartesian_trajectory_planner_B.time;
21560 }
21561
21562 cartesian_trajectory_planner_B.tfCalc[12] =
21563 cartesian_trajectory_planner_B.tfCalc_tmp[0] *
21564 cartesian_trajectory_planner_B.b_f1[0] + cartesian_trajectory_planner_B.a_p
21565 [0];
21566 cartesian_trajectory_planner_B.vCalc[0] = cartesian_trajectory_planner_B.w[0];
21567 cartesian_trajectory_planner_B.vCalc[3] =
21568 cartesian_trajectory_planner_B.tfCalc_tmp[0] *
21569 cartesian_trajectory_planner_B.b_f1[1];
21570 cartesian_trajectory_planner_B.tfCalc[13] =
21571 cartesian_trajectory_planner_B.tfCalc_tmp[1] *
21572 cartesian_trajectory_planner_B.b_f1[0] + cartesian_trajectory_planner_B.a_p
21573 [1];
21574 cartesian_trajectory_planner_B.vCalc[1] = cartesian_trajectory_planner_B.w[1];
21575 cartesian_trajectory_planner_B.vCalc[4] =
21576 cartesian_trajectory_planner_B.tfCalc_tmp[1] *
21577 cartesian_trajectory_planner_B.b_f1[1];
21578 cartesian_trajectory_planner_B.tfCalc[14] =
21579 cartesian_trajectory_planner_B.tfCalc_tmp[2] *
21580 cartesian_trajectory_planner_B.b_f1[0] + cartesian_trajectory_planner_B.a_p
21581 [2];
21582 cartesian_trajectory_planner_B.vCalc[2] = cartesian_trajectory_planner_B.w[2];
21583 cartesian_trajectory_planner_B.vCalc[5] =
21584 cartesian_trajectory_planner_B.tfCalc_tmp[2] *
21585 cartesian_trajectory_planner_B.b_f1[1];
21586 for (cartesian_trajectory_planner_B.b_k_e = 0;
21587 cartesian_trajectory_planner_B.b_k_e < 4;
21588 cartesian_trajectory_planner_B.b_k_e++) {
21589 cartesian_trajectory_planner_B.subsa_idx_1 =
21590 cartesian_trajectory_planner_B.b_k_e << 2;
21591 cartesian_trajectory_planner_B.out[cartesian_trajectory_planner_B.subsa_idx_1]
21592 =
21593 cartesian_trajectory_planner_B.tfCalc[cartesian_trajectory_planner_B.subsa_idx_1];
21594 cartesian_trajectory_planner_B.out_tmp =
21595 cartesian_trajectory_planner_B.subsa_idx_1 + 1;
21596 cartesian_trajectory_planner_B.out[cartesian_trajectory_planner_B.out_tmp] =
21597 cartesian_trajectory_planner_B.tfCalc[cartesian_trajectory_planner_B.out_tmp];
21598 cartesian_trajectory_planner_B.out_tmp =
21599 cartesian_trajectory_planner_B.subsa_idx_1 + 2;
21600 cartesian_trajectory_planner_B.out[cartesian_trajectory_planner_B.out_tmp] =
21601 cartesian_trajectory_planner_B.tfCalc[cartesian_trajectory_planner_B.out_tmp];
21602 cartesian_trajectory_planner_B.subsa_idx_1 += 3;
21603 cartesian_trajectory_planner_B.out[cartesian_trajectory_planner_B.subsa_idx_1]
21604 =
21605 cartesian_trajectory_planner_B.tfCalc[cartesian_trajectory_planner_B.subsa_idx_1];
21606 }
21607
21608 for (cartesian_trajectory_planner_B.b_k_e = 0;
21609 cartesian_trajectory_planner_B.b_k_e < 6;
21610 cartesian_trajectory_planner_B.b_k_e++) {
21611 cartesian_trajectory_planner_B.b_varargout_2[cartesian_trajectory_planner_B.b_k_e]
21612 =
21613 cartesian_trajectory_planner_B.vCalc[cartesian_trajectory_planner_B.b_k_e];
21614 }
21615
21616 // MATLABSystem: '<S3>/MATLAB System' incorporates:
21617 // MATLABSystem: '<S9>/Get Parameter'
21618 // MATLABSystem: '<S9>/Get Parameter1'
21619 // MATLABSystem: '<S9>/Get Parameter2'
21620 // MATLABSystem: '<S9>/Get Parameter3'
21621 // MATLABSystem: '<S9>/Get Parameter4'
21622 // MATLABSystem: '<S9>/Get Parameter5'
21623
21624 ParamGet_cartesian_trajectory_planner_301.get_parameter
21625 (&cartesian_trajectory_planner_B.value[0]);
21626 ParamGet_cartesian_trajectory_planner_302.get_parameter
21627 (&cartesian_trajectory_planner_B.value[1]);
21628 ParamGet_cartesian_trajectory_planner_303.get_parameter
21629 (&cartesian_trajectory_planner_B.value[2]);
21630 ParamGet_cartesian_trajectory_planner_304.get_parameter
21631 (&cartesian_trajectory_planner_B.value[3]);
21632 ParamGet_cartesian_trajectory_planner_305.get_parameter
21633 (&cartesian_trajectory_planner_B.value[4]);
21634 ParamGet_cartesian_trajectory_planner_306.get_parameter
21635 (&cartesian_trajectory_planner_B.value[5]);
21636 obj = &cartesian_trajectory_planner_DW.obj;
21637 if (cartesian_trajectory_planner_DW.obj.IKInternal.isInitialized != 1) {
21638 cartesian_trajectory_planner_DW.obj.IKInternal.isSetupComplete = false;
21639 cartesian_trajectory_planner_DW.obj.IKInternal.isInitialized = 1;
21640 car_inverseKinematics_setupImpl
21641 (&cartesian_trajectory_planner_DW.obj.IKInternal,
21642 &cartesian_trajectory_planner_DW.gobj_85);
21643 obj->IKInternal.isSetupComplete = true;
21644 }
21645
21646 cartesian_trajec_emxInit_real_T(&b, 2);
21647
21648 // Delay: '<Root>/Delay'
21649 for (cartesian_trajectory_planner_B.b_k_e = 0;
21650 cartesian_trajectory_planner_B.b_k_e < 6;
21651 cartesian_trajectory_planner_B.b_k_e++) {
21652 cartesian_trajectory_planner_B.dv1[cartesian_trajectory_planner_B.b_k_e] =
21653 cartesian_trajectory_planner_DW.Delay_DSTATE[cartesian_trajectory_planner_B.b_k_e];
21654 }
21655
21656 // MATLABSystem: '<S3>/MATLAB System' incorporates:
21657 // Delay: '<Root>/Delay'
21658 // MATLABSystem: '<Root>/Transform Trajectory'
21659
21660 cart_inverseKinematics_stepImpl(&obj->IKInternal,
21661 cartesian_trajectory_planner_B.out, cartesian_trajectory_planner_B.value,
21662 cartesian_trajectory_planner_B.dv1,
21663 cartesian_trajectory_planner_DW.Delay_DSTATE);
21664
21665 // MATLABSystem: '<S2>/MATLAB System' incorporates:
21666 // Delay: '<Root>/Delay'
21667
21668 RigidBodyTree_geometricJacobian
21669 (&cartesian_trajectory_planner_DW.obj_o.TreeInternal,
21670 cartesian_trajectory_planner_DW.Delay_DSTATE, b);
21671
21672 // MATLAB Function: '<Root>/MATLAB Function' incorporates:
21673 // Constant: '<S1>/Constant'
21674
21675 cartesian_trajectory_planner_B.msg =
21676 cartesian_trajectory_planner_P.Constant_Value;
21677 cartesian_trajectory_planner_B.msg.Velocities_SL_Info.CurrentLength = 6U;
21678
21679 // Product: '<Root>/Reciprocal' incorporates:
21680 // MATLABSystem: '<S2>/MATLAB System'
21681
21682 rt_invd6x6_snf(b->data, cartesian_trajectory_planner_B.dv);
21683 cartesian_trajec_emxFree_real_T(&b);
21684
21685 // MATLAB Function: '<Root>/MATLAB Function'
21686 cartesian_trajectory_planner_B.msg.Positions_SL_Info.CurrentLength = 6U;
21687 for (cartesian_trajectory_planner_B.subsa_idx_1 = 0;
21688 cartesian_trajectory_planner_B.subsa_idx_1 < 6;
21689 cartesian_trajectory_planner_B.subsa_idx_1++) {
21690 // Product: '<Root>/MatrixMultiply1' incorporates:
21691 // MATLABSystem: '<Root>/Transform Trajectory'
21692 // Product: '<Root>/Reciprocal'
21693
21694 cartesian_trajectory_planner_B.vCalc[cartesian_trajectory_planner_B.subsa_idx_1]
21695 = 0.0;
21696 for (cartesian_trajectory_planner_B.b_k_e = 0;
21697 cartesian_trajectory_planner_B.b_k_e < 6;
21698 cartesian_trajectory_planner_B.b_k_e++) {
21699 cartesian_trajectory_planner_B.vCalc[cartesian_trajectory_planner_B.subsa_idx_1]
21700 += cartesian_trajectory_planner_B.dv[6 *
21701 cartesian_trajectory_planner_B.b_k_e +
21702 cartesian_trajectory_planner_B.subsa_idx_1] *
21703 cartesian_trajectory_planner_B.b_varargout_2[cartesian_trajectory_planner_B.b_k_e];
21704 }
21705
21706 // End of Product: '<Root>/MatrixMultiply1'
21707
21708 // MATLAB Function: '<Root>/MATLAB Function' incorporates:
21709 // Delay: '<Root>/Delay'
21710
21711 cartesian_trajectory_planner_B.msg.Velocities[cartesian_trajectory_planner_B.subsa_idx_1]
21712 =
21713 cartesian_trajectory_planner_B.vCalc[cartesian_trajectory_planner_B.subsa_idx_1];
21714 cartesian_trajectory_planner_B.msg.Positions[cartesian_trajectory_planner_B.subsa_idx_1]
21715 =
21716 cartesian_trajectory_planner_DW.Delay_DSTATE[cartesian_trajectory_planner_B.subsa_idx_1];
21717 }
21718
21719 // Outputs for Atomic SubSystem: '<Root>/Publish'
21720 // MATLABSystem: '<S6>/SinkBlock'
21721 Pub_cartesian_trajectory_planner_267.publish
21722 (&cartesian_trajectory_planner_B.msg);
21723
21724 // End of Outputs for SubSystem: '<Root>/Publish'
21725}
21726
21727// Model initialize function
21728void cartesian_trajectory_planner_initialize(void)
21729{
21730 // Registration code
21731
21732 // initialize non-finites
21733 rt_InitInfAndNaN(sizeof(real_T));
21734
21735 {
21736 robotics_slmanip_internal_b_a_T *obj;
21737 b_inverseKinematics_cartesian_T *obj_0;
21738 h_robotics_core_internal_Damp_T *obj_1;
21739 static const real_T tmp[33] = { 0.0, 1.0, 0.0, 0.1, 1.0, 0.0, 0.2, 1.0, 0.0,
21740 0.30000000000000004, 1.0, 0.0, 0.4, 1.0, 0.0, 0.5, 1.0, 0.0, 0.6, 1.0, 0.0,
21741 0.7, 1.0, 0.0, 0.8, 1.0, 0.0, 0.9, 1.0, 0.0, 1.0, 1.0, 0.0 };
21742
21743 static const char_T tmp_0[6] = { '/', 'c', 'l', 'o', 'c', 'k' };
21744
21745 static const char_T tmp_1[17] = { '/', 'j', 'o', 'i', 'n', 't', '_', 't',
21746 'r', 'a', 'j', 'e', 'c', 't', 'o', 'r', 'y' };
21747
21748 static const char_T tmp_2[12] = { '/', 's', 't', 'a', 'r', 't', '_', 'd',
21749 'e', 'l', 'a', 'y' };
21750
21751 static const char_T tmp_3[5] = { '/', 'o', 'i', '_', 'x' };
21752
21753 static const char_T tmp_4[5] = { '/', 'o', 'i', '_', 'y' };
21754
21755 static const char_T tmp_5[5] = { '/', 'o', 'i', '_', 'z' };
21756
21757 static const char_T tmp_6[5] = { '/', 'o', 'i', '_', 'w' };
21758
21759 static const char_T tmp_7[5] = { '/', 'p', 'i', '_', 'x' };
21760
21761 static const char_T tmp_8[5] = { '/', 'p', 'i', '_', 'y' };
21762
21763 static const char_T tmp_9[5] = { '/', 'p', 'i', '_', 'z' };
21764
21765 static const char_T tmp_a[5] = { '/', 'o', 'f', '_', 'x' };
21766
21767 static const char_T tmp_b[5] = { '/', 'o', 'f', '_', 'y' };
21768
21769 static const char_T tmp_c[5] = { '/', 'o', 'f', '_', 'z' };
21770
21771 static const char_T tmp_d[5] = { '/', 'o', 'f', '_', 'w' };
21772
21773 static const char_T tmp_e[5] = { '/', 'p', 'f', '_', 'x' };
21774
21775 static const char_T tmp_f[5] = { '/', 'p', 'f', '_', 'y' };
21776
21777 static const char_T tmp_g[5] = { '/', 'p', 'f', '_', 'z' };
21778
21779 static const char_T tmp_h[10] = { '/', 'w', 'e', 'i', 'g', 'h', 't', '_',
21780 'o', 'x' };
21781
21782 static const char_T tmp_i[10] = { '/', 'w', 'e', 'i', 'g', 'h', 't', '_',
21783 'o', 'y' };
21784
21785 static const char_T tmp_j[10] = { '/', 'w', 'e', 'i', 'g', 'h', 't', '_',
21786 'o', 'z' };
21787
21788 static const char_T tmp_k[10] = { '/', 'w', 'e', 'i', 'g', 'h', 't', '_',
21789 'p', 'x' };
21790
21791 static const char_T tmp_l[10] = { '/', 'w', 'e', 'i', 'g', 'h', 't', '_',
21792 'p', 'y' };
21793
21794 static const char_T tmp_m[10] = { '/', 'w', 'e', 'i', 'g', 'h', 't', '_',
21795 'p', 'z' };
21796
21797 // SystemInitialize for Atomic SubSystem: '<Root>/Subscribe'
21798 // SystemInitialize for Enabled SubSystem: '<S7>/Enabled Subsystem'
21799 // SystemInitialize for Outport: '<S10>/Out1'
21800 cartesian_trajectory_planner_B.In1 = cartesian_trajectory_planner_P.Out1_Y0;
21801
21802 // End of SystemInitialize for SubSystem: '<S7>/Enabled Subsystem'
21803
21804 // Start for MATLABSystem: '<S7>/SourceBlock'
21805 cartesian_trajectory_planner_DW.obj_j.matlabCodegenIsDeleted = false;
21806 cartesian_trajectory_planner_DW.obj_j.isInitialized = 1;
21807 for (cartesian_trajectory_planner_B.i_mq = 0;
21808 cartesian_trajectory_planner_B.i_mq < 6;
21809 cartesian_trajectory_planner_B.i_mq++) {
21810 // InitializeConditions for Delay: '<Root>/Delay'
21811 cartesian_trajectory_planner_DW.Delay_DSTATE[cartesian_trajectory_planner_B.i_mq]
21812 =
21813 cartesian_trajectory_planner_P.Delay_InitialCondition[cartesian_trajectory_planner_B.i_mq];
21814
21815 // Start for MATLABSystem: '<S7>/SourceBlock'
21816 cartesian_trajectory_planner_B.cv3[cartesian_trajectory_planner_B.i_mq] =
21817 tmp_0[cartesian_trajectory_planner_B.i_mq];
21818 }
21819
21820 // Start for MATLABSystem: '<S7>/SourceBlock'
21821 cartesian_trajectory_planner_B.cv3[6] = '\x00';
21822 Sub_cartesian_trajectory_planner_269.createSubscriber
21823 (cartesian_trajectory_planner_B.cv3, 1);
21824 cartesian_trajectory_planner_DW.obj_j.isSetupComplete = true;
21825
21826 // End of SystemInitialize for SubSystem: '<Root>/Subscribe'
21827
21828 // SystemInitialize for Atomic SubSystem: '<Root>/Publish'
21829 // Start for MATLABSystem: '<S6>/SinkBlock'
21830 cartesian_trajectory_planner_DW.obj_kh.matlabCodegenIsDeleted = false;
21831 cartesian_trajectory_planner_DW.obj_kh.isInitialized = 1;
21832 for (cartesian_trajectory_planner_B.i_mq = 0;
21833 cartesian_trajectory_planner_B.i_mq < 17;
21834 cartesian_trajectory_planner_B.i_mq++) {
21835 cartesian_trajectory_planner_B.cv[cartesian_trajectory_planner_B.i_mq] =
21836 tmp_1[cartesian_trajectory_planner_B.i_mq];
21837 }
21838
21839 cartesian_trajectory_planner_B.cv[17] = '\x00';
21840 Pub_cartesian_trajectory_planner_267.createPublisher
21841 (cartesian_trajectory_planner_B.cv, 1);
21842 cartesian_trajectory_planner_DW.obj_kh.isSetupComplete = true;
21843
21844 // End of Start for MATLABSystem: '<S6>/SinkBlock'
21845 // End of SystemInitialize for SubSystem: '<Root>/Publish'
21846
21847 // Start for MATLABSystem: '<S8>/Get Parameter'
21848 cartesian_trajectory_planner_DW.obj_k.matlabCodegenIsDeleted = false;
21849 cartesian_trajectory_planner_DW.obj_k.isInitialized = 1;
21850 for (cartesian_trajectory_planner_B.i_mq = 0;
21851 cartesian_trajectory_planner_B.i_mq < 12;
21852 cartesian_trajectory_planner_B.i_mq++) {
21853 cartesian_trajectory_planner_B.cv1[cartesian_trajectory_planner_B.i_mq] =
21854 tmp_2[cartesian_trajectory_planner_B.i_mq];
21855 }
21856
21857 cartesian_trajectory_planner_B.cv1[12] = '\x00';
21858 ParamGet_cartesian_trajectory_planner_272.initialize
21859 (cartesian_trajectory_planner_B.cv1);
21860 ParamGet_cartesian_trajectory_planner_272.initialize_error_codes(0, 1, 2, 3);
21861 ParamGet_cartesian_trajectory_planner_272.set_initial_value(40.0);
21862 cartesian_trajectory_planner_DW.obj_k.isSetupComplete = true;
21863
21864 // End of Start for MATLABSystem: '<S8>/Get Parameter'
21865
21866 // Start for MATLABSystem: '<S8>/Get Parameter1'
21867 cartesian_trajectory_planner_DW.obj_ky.matlabCodegenIsDeleted = false;
21868 cartesian_trajectory_planner_DW.obj_ky.isInitialized = 1;
21869 cartesian_trajectory_planner_B.cv5[0] = '/';
21870 cartesian_trajectory_planner_B.cv5[1] = 't';
21871 cartesian_trajectory_planner_B.cv5[2] = 'f';
21872 cartesian_trajectory_planner_B.cv5[3] = '\x00';
21873 ParamGet_cartesian_trajectory_planner_273.initialize
21874 (cartesian_trajectory_planner_B.cv5);
21875 ParamGet_cartesian_trajectory_planner_273.initialize_error_codes(0, 1, 2, 3);
21876 ParamGet_cartesian_trajectory_planner_273.set_initial_value(70.0);
21877 cartesian_trajectory_planner_DW.obj_ky.isSetupComplete = true;
21878
21879 // Start for MATLABSystem: '<S11>/Get Parameter3'
21880 cartesian_trajectory_planner_DW.obj_g.matlabCodegenIsDeleted = false;
21881 cartesian_trajectory_planner_DW.obj_g.isInitialized = 1;
21882 for (cartesian_trajectory_planner_B.i_mq = 0;
21883 cartesian_trajectory_planner_B.i_mq < 5;
21884 cartesian_trajectory_planner_B.i_mq++) {
21885 cartesian_trajectory_planner_B.cv4[cartesian_trajectory_planner_B.i_mq] =
21886 tmp_3[cartesian_trajectory_planner_B.i_mq];
21887 }
21888
21889 cartesian_trajectory_planner_B.cv4[5] = '\x00';
21890 ParamGet_cartesian_trajectory_planner_280.initialize
21891 (cartesian_trajectory_planner_B.cv4);
21892 ParamGet_cartesian_trajectory_planner_280.initialize_error_codes(0, 1, 2, 3);
21893 ParamGet_cartesian_trajectory_planner_280.set_initial_value(0.0);
21894 cartesian_trajectory_planner_DW.obj_g.isSetupComplete = true;
21895
21896 // End of Start for MATLABSystem: '<S11>/Get Parameter3'
21897
21898 // Start for MATLABSystem: '<S11>/Get Parameter4'
21899 cartesian_trajectory_planner_DW.obj_or.matlabCodegenIsDeleted = false;
21900 cartesian_trajectory_planner_DW.obj_or.isInitialized = 1;
21901 for (cartesian_trajectory_planner_B.i_mq = 0;
21902 cartesian_trajectory_planner_B.i_mq < 5;
21903 cartesian_trajectory_planner_B.i_mq++) {
21904 cartesian_trajectory_planner_B.cv4[cartesian_trajectory_planner_B.i_mq] =
21905 tmp_4[cartesian_trajectory_planner_B.i_mq];
21906 }
21907
21908 cartesian_trajectory_planner_B.cv4[5] = '\x00';
21909 ParamGet_cartesian_trajectory_planner_281.initialize
21910 (cartesian_trajectory_planner_B.cv4);
21911 ParamGet_cartesian_trajectory_planner_281.initialize_error_codes(0, 1, 2, 3);
21912 ParamGet_cartesian_trajectory_planner_281.set_initial_value(0.0);
21913 cartesian_trajectory_planner_DW.obj_or.isSetupComplete = true;
21914
21915 // End of Start for MATLABSystem: '<S11>/Get Parameter4'
21916
21917 // Start for MATLABSystem: '<S11>/Get Parameter5'
21918 cartesian_trajectory_planner_DW.obj_dz.matlabCodegenIsDeleted = false;
21919 cartesian_trajectory_planner_DW.obj_dz.isInitialized = 1;
21920 for (cartesian_trajectory_planner_B.i_mq = 0;
21921 cartesian_trajectory_planner_B.i_mq < 5;
21922 cartesian_trajectory_planner_B.i_mq++) {
21923 cartesian_trajectory_planner_B.cv4[cartesian_trajectory_planner_B.i_mq] =
21924 tmp_5[cartesian_trajectory_planner_B.i_mq];
21925 }
21926
21927 cartesian_trajectory_planner_B.cv4[5] = '\x00';
21928 ParamGet_cartesian_trajectory_planner_282.initialize
21929 (cartesian_trajectory_planner_B.cv4);
21930 ParamGet_cartesian_trajectory_planner_282.initialize_error_codes(0, 1, 2, 3);
21931 ParamGet_cartesian_trajectory_planner_282.set_initial_value(0.0);
21932 cartesian_trajectory_planner_DW.obj_dz.isSetupComplete = true;
21933
21934 // End of Start for MATLABSystem: '<S11>/Get Parameter5'
21935
21936 // Start for MATLABSystem: '<S11>/Get Parameter6'
21937 cartesian_trajectory_planner_DW.obj_di.matlabCodegenIsDeleted = false;
21938 cartesian_trajectory_planner_DW.obj_di.isInitialized = 1;
21939 for (cartesian_trajectory_planner_B.i_mq = 0;
21940 cartesian_trajectory_planner_B.i_mq < 5;
21941 cartesian_trajectory_planner_B.i_mq++) {
21942 cartesian_trajectory_planner_B.cv4[cartesian_trajectory_planner_B.i_mq] =
21943 tmp_6[cartesian_trajectory_planner_B.i_mq];
21944 }
21945
21946 cartesian_trajectory_planner_B.cv4[5] = '\x00';
21947 ParamGet_cartesian_trajectory_planner_283.initialize
21948 (cartesian_trajectory_planner_B.cv4);
21949 ParamGet_cartesian_trajectory_planner_283.initialize_error_codes(0, 1, 2, 3);
21950 ParamGet_cartesian_trajectory_planner_283.set_initial_value(1.0);
21951 cartesian_trajectory_planner_DW.obj_di.isSetupComplete = true;
21952
21953 // End of Start for MATLABSystem: '<S11>/Get Parameter6'
21954
21955 // Start for MATLABSystem: '<S11>/Get Parameter'
21956 cartesian_trajectory_planner_DW.obj_b.matlabCodegenIsDeleted = false;
21957 cartesian_trajectory_planner_DW.obj_b.isInitialized = 1;
21958 for (cartesian_trajectory_planner_B.i_mq = 0;
21959 cartesian_trajectory_planner_B.i_mq < 5;
21960 cartesian_trajectory_planner_B.i_mq++) {
21961 cartesian_trajectory_planner_B.cv4[cartesian_trajectory_planner_B.i_mq] =
21962 tmp_7[cartesian_trajectory_planner_B.i_mq];
21963 }
21964
21965 cartesian_trajectory_planner_B.cv4[5] = '\x00';
21966 ParamGet_cartesian_trajectory_planner_277.initialize
21967 (cartesian_trajectory_planner_B.cv4);
21968 ParamGet_cartesian_trajectory_planner_277.initialize_error_codes(0, 1, 2, 3);
21969 ParamGet_cartesian_trajectory_planner_277.set_initial_value(0.0);
21970 cartesian_trajectory_planner_DW.obj_b.isSetupComplete = true;
21971
21972 // End of Start for MATLABSystem: '<S11>/Get Parameter'
21973
21974 // Start for MATLABSystem: '<S11>/Get Parameter1'
21975 cartesian_trajectory_planner_DW.obj_e0t.matlabCodegenIsDeleted = false;
21976 cartesian_trajectory_planner_DW.obj_e0t.isInitialized = 1;
21977 for (cartesian_trajectory_planner_B.i_mq = 0;
21978 cartesian_trajectory_planner_B.i_mq < 5;
21979 cartesian_trajectory_planner_B.i_mq++) {
21980 cartesian_trajectory_planner_B.cv4[cartesian_trajectory_planner_B.i_mq] =
21981 tmp_8[cartesian_trajectory_planner_B.i_mq];
21982 }
21983
21984 cartesian_trajectory_planner_B.cv4[5] = '\x00';
21985 ParamGet_cartesian_trajectory_planner_278.initialize
21986 (cartesian_trajectory_planner_B.cv4);
21987 ParamGet_cartesian_trajectory_planner_278.initialize_error_codes(0, 1, 2, 3);
21988 ParamGet_cartesian_trajectory_planner_278.set_initial_value(0.0);
21989 cartesian_trajectory_planner_DW.obj_e0t.isSetupComplete = true;
21990
21991 // End of Start for MATLABSystem: '<S11>/Get Parameter1'
21992
21993 // Start for MATLABSystem: '<S11>/Get Parameter2'
21994 cartesian_trajectory_planner_DW.obj_pd.matlabCodegenIsDeleted = false;
21995 cartesian_trajectory_planner_DW.obj_pd.isInitialized = 1;
21996 for (cartesian_trajectory_planner_B.i_mq = 0;
21997 cartesian_trajectory_planner_B.i_mq < 5;
21998 cartesian_trajectory_planner_B.i_mq++) {
21999 cartesian_trajectory_planner_B.cv4[cartesian_trajectory_planner_B.i_mq] =
22000 tmp_9[cartesian_trajectory_planner_B.i_mq];
22001 }
22002
22003 cartesian_trajectory_planner_B.cv4[5] = '\x00';
22004 ParamGet_cartesian_trajectory_planner_279.initialize
22005 (cartesian_trajectory_planner_B.cv4);
22006 ParamGet_cartesian_trajectory_planner_279.initialize_error_codes(0, 1, 2, 3);
22007 ParamGet_cartesian_trajectory_planner_279.set_initial_value(0.99);
22008 cartesian_trajectory_planner_DW.obj_pd.isSetupComplete = true;
22009
22010 // End of Start for MATLABSystem: '<S11>/Get Parameter2'
22011
22012 // Start for MATLABSystem: '<S12>/Get Parameter3'
22013 cartesian_trajectory_planner_DW.obj_f4.matlabCodegenIsDeleted = false;
22014 cartesian_trajectory_planner_DW.obj_f4.isInitialized = 1;
22015 for (cartesian_trajectory_planner_B.i_mq = 0;
22016 cartesian_trajectory_planner_B.i_mq < 5;
22017 cartesian_trajectory_planner_B.i_mq++) {
22018 cartesian_trajectory_planner_B.cv4[cartesian_trajectory_planner_B.i_mq] =
22019 tmp_a[cartesian_trajectory_planner_B.i_mq];
22020 }
22021
22022 cartesian_trajectory_planner_B.cv4[5] = '\x00';
22023 ParamGet_cartesian_trajectory_planner_292.initialize
22024 (cartesian_trajectory_planner_B.cv4);
22025 ParamGet_cartesian_trajectory_planner_292.initialize_error_codes(0, 1, 2, 3);
22026 ParamGet_cartesian_trajectory_planner_292.set_initial_value(0.39);
22027 cartesian_trajectory_planner_DW.obj_f4.isSetupComplete = true;
22028
22029 // End of Start for MATLABSystem: '<S12>/Get Parameter3'
22030
22031 // Start for MATLABSystem: '<S12>/Get Parameter4'
22032 cartesian_trajectory_planner_DW.obj_en.matlabCodegenIsDeleted = false;
22033 cartesian_trajectory_planner_DW.obj_en.isInitialized = 1;
22034 for (cartesian_trajectory_planner_B.i_mq = 0;
22035 cartesian_trajectory_planner_B.i_mq < 5;
22036 cartesian_trajectory_planner_B.i_mq++) {
22037 cartesian_trajectory_planner_B.cv4[cartesian_trajectory_planner_B.i_mq] =
22038 tmp_b[cartesian_trajectory_planner_B.i_mq];
22039 }
22040
22041 cartesian_trajectory_planner_B.cv4[5] = '\x00';
22042 ParamGet_cartesian_trajectory_planner_293.initialize
22043 (cartesian_trajectory_planner_B.cv4);
22044 ParamGet_cartesian_trajectory_planner_293.initialize_error_codes(0, 1, 2, 3);
22045 ParamGet_cartesian_trajectory_planner_293.set_initial_value(0.89);
22046 cartesian_trajectory_planner_DW.obj_en.isSetupComplete = true;
22047
22048 // End of Start for MATLABSystem: '<S12>/Get Parameter4'
22049
22050 // Start for MATLABSystem: '<S12>/Get Parameter5'
22051 cartesian_trajectory_planner_DW.obj_l0.matlabCodegenIsDeleted = false;
22052 cartesian_trajectory_planner_DW.obj_l0.isInitialized = 1;
22053 for (cartesian_trajectory_planner_B.i_mq = 0;
22054 cartesian_trajectory_planner_B.i_mq < 5;
22055 cartesian_trajectory_planner_B.i_mq++) {
22056 cartesian_trajectory_planner_B.cv4[cartesian_trajectory_planner_B.i_mq] =
22057 tmp_c[cartesian_trajectory_planner_B.i_mq];
22058 }
22059
22060 cartesian_trajectory_planner_B.cv4[5] = '\x00';
22061 ParamGet_cartesian_trajectory_planner_294.initialize
22062 (cartesian_trajectory_planner_B.cv4);
22063 ParamGet_cartesian_trajectory_planner_294.initialize_error_codes(0, 1, 2, 3);
22064 ParamGet_cartesian_trajectory_planner_294.set_initial_value(0.2);
22065 cartesian_trajectory_planner_DW.obj_l0.isSetupComplete = true;
22066
22067 // End of Start for MATLABSystem: '<S12>/Get Parameter5'
22068
22069 // Start for MATLABSystem: '<S12>/Get Parameter6'
22070 cartesian_trajectory_planner_DW.obj_n.matlabCodegenIsDeleted = false;
22071 cartesian_trajectory_planner_DW.obj_n.isInitialized = 1;
22072 for (cartesian_trajectory_planner_B.i_mq = 0;
22073 cartesian_trajectory_planner_B.i_mq < 5;
22074 cartesian_trajectory_planner_B.i_mq++) {
22075 cartesian_trajectory_planner_B.cv4[cartesian_trajectory_planner_B.i_mq] =
22076 tmp_d[cartesian_trajectory_planner_B.i_mq];
22077 }
22078
22079 cartesian_trajectory_planner_B.cv4[5] = '\x00';
22080 ParamGet_cartesian_trajectory_planner_295.initialize
22081 (cartesian_trajectory_planner_B.cv4);
22082 ParamGet_cartesian_trajectory_planner_295.initialize_error_codes(0, 1, 2, 3);
22083 ParamGet_cartesian_trajectory_planner_295.set_initial_value(-0.12);
22084 cartesian_trajectory_planner_DW.obj_n.isSetupComplete = true;
22085
22086 // End of Start for MATLABSystem: '<S12>/Get Parameter6'
22087
22088 // Start for MATLABSystem: '<S12>/Get Parameter'
22089 cartesian_trajectory_planner_DW.obj_d.matlabCodegenIsDeleted = false;
22090 cartesian_trajectory_planner_DW.obj_d.isInitialized = 1;
22091 for (cartesian_trajectory_planner_B.i_mq = 0;
22092 cartesian_trajectory_planner_B.i_mq < 5;
22093 cartesian_trajectory_planner_B.i_mq++) {
22094 cartesian_trajectory_planner_B.cv4[cartesian_trajectory_planner_B.i_mq] =
22095 tmp_e[cartesian_trajectory_planner_B.i_mq];
22096 }
22097
22098 cartesian_trajectory_planner_B.cv4[5] = '\x00';
22099 ParamGet_cartesian_trajectory_planner_289.initialize
22100 (cartesian_trajectory_planner_B.cv4);
22101 ParamGet_cartesian_trajectory_planner_289.initialize_error_codes(0, 1, 2, 3);
22102 ParamGet_cartesian_trajectory_planner_289.set_initial_value(0.21);
22103 cartesian_trajectory_planner_DW.obj_d.isSetupComplete = true;
22104
22105 // End of Start for MATLABSystem: '<S12>/Get Parameter'
22106
22107 // Start for MATLABSystem: '<S12>/Get Parameter1'
22108 cartesian_trajectory_planner_DW.obj_oq.matlabCodegenIsDeleted = false;
22109 cartesian_trajectory_planner_DW.obj_oq.isInitialized = 1;
22110 for (cartesian_trajectory_planner_B.i_mq = 0;
22111 cartesian_trajectory_planner_B.i_mq < 5;
22112 cartesian_trajectory_planner_B.i_mq++) {
22113 cartesian_trajectory_planner_B.cv4[cartesian_trajectory_planner_B.i_mq] =
22114 tmp_f[cartesian_trajectory_planner_B.i_mq];
22115 }
22116
22117 cartesian_trajectory_planner_B.cv4[5] = '\x00';
22118 ParamGet_cartesian_trajectory_planner_290.initialize
22119 (cartesian_trajectory_planner_B.cv4);
22120 ParamGet_cartesian_trajectory_planner_290.initialize_error_codes(0, 1, 2, 3);
22121 ParamGet_cartesian_trajectory_planner_290.set_initial_value(0.89);
22122 cartesian_trajectory_planner_DW.obj_oq.isSetupComplete = true;
22123
22124 // End of Start for MATLABSystem: '<S12>/Get Parameter1'
22125
22126 // Start for MATLABSystem: '<S12>/Get Parameter2'
22127 cartesian_trajectory_planner_DW.obj_es.matlabCodegenIsDeleted = false;
22128 cartesian_trajectory_planner_DW.obj_es.isInitialized = 1;
22129 for (cartesian_trajectory_planner_B.i_mq = 0;
22130 cartesian_trajectory_planner_B.i_mq < 5;
22131 cartesian_trajectory_planner_B.i_mq++) {
22132 cartesian_trajectory_planner_B.cv4[cartesian_trajectory_planner_B.i_mq] =
22133 tmp_g[cartesian_trajectory_planner_B.i_mq];
22134 }
22135
22136 cartesian_trajectory_planner_B.cv4[5] = '\x00';
22137 ParamGet_cartesian_trajectory_planner_291.initialize
22138 (cartesian_trajectory_planner_B.cv4);
22139 ParamGet_cartesian_trajectory_planner_291.initialize_error_codes(0, 1, 2, 3);
22140 ParamGet_cartesian_trajectory_planner_291.set_initial_value(0.2);
22141 cartesian_trajectory_planner_DW.obj_es.isSetupComplete = true;
22142
22143 // End of Start for MATLABSystem: '<S12>/Get Parameter2'
22144
22145 // Start for MATLABSystem: '<Root>/Transform Trajectory'
22146 memcpy(&cartesian_trajectory_planner_DW.obj_e.TimeScaling[0], &tmp[0], 33U *
22147 sizeof(real_T));
22148 cartesian_trajectory_planner_DW.obj_e.isInitialized = 0;
22149 cartesian_traj_SystemCore_setup(&cartesian_trajectory_planner_DW.obj_e);
22150
22151 // Start for MATLABSystem: '<S9>/Get Parameter'
22152 cartesian_trajectory_planner_DW.obj_c.matlabCodegenIsDeleted = false;
22153 cartesian_trajectory_planner_DW.obj_c.isInitialized = 1;
22154 for (cartesian_trajectory_planner_B.i_mq = 0;
22155 cartesian_trajectory_planner_B.i_mq < 10;
22156 cartesian_trajectory_planner_B.i_mq++) {
22157 cartesian_trajectory_planner_B.cv2[cartesian_trajectory_planner_B.i_mq] =
22158 tmp_h[cartesian_trajectory_planner_B.i_mq];
22159 }
22160
22161 cartesian_trajectory_planner_B.cv2[10] = '\x00';
22162 ParamGet_cartesian_trajectory_planner_301.initialize
22163 (cartesian_trajectory_planner_B.cv2);
22164 ParamGet_cartesian_trajectory_planner_301.initialize_error_codes(0, 1, 2, 3);
22165 ParamGet_cartesian_trajectory_planner_301.set_initial_value(0.0);
22166 cartesian_trajectory_planner_DW.obj_c.isSetupComplete = true;
22167
22168 // End of Start for MATLABSystem: '<S9>/Get Parameter'
22169
22170 // Start for MATLABSystem: '<S9>/Get Parameter1'
22171 cartesian_trajectory_planner_DW.obj_f.matlabCodegenIsDeleted = false;
22172 cartesian_trajectory_planner_DW.obj_f.isInitialized = 1;
22173 for (cartesian_trajectory_planner_B.i_mq = 0;
22174 cartesian_trajectory_planner_B.i_mq < 10;
22175 cartesian_trajectory_planner_B.i_mq++) {
22176 cartesian_trajectory_planner_B.cv2[cartesian_trajectory_planner_B.i_mq] =
22177 tmp_i[cartesian_trajectory_planner_B.i_mq];
22178 }
22179
22180 cartesian_trajectory_planner_B.cv2[10] = '\x00';
22181 ParamGet_cartesian_trajectory_planner_302.initialize
22182 (cartesian_trajectory_planner_B.cv2);
22183 ParamGet_cartesian_trajectory_planner_302.initialize_error_codes(0, 1, 2, 3);
22184 ParamGet_cartesian_trajectory_planner_302.set_initial_value(0.0);
22185 cartesian_trajectory_planner_DW.obj_f.isSetupComplete = true;
22186
22187 // End of Start for MATLABSystem: '<S9>/Get Parameter1'
22188
22189 // Start for MATLABSystem: '<S9>/Get Parameter2'
22190 cartesian_trajectory_planner_DW.obj_e0.matlabCodegenIsDeleted = false;
22191 cartesian_trajectory_planner_DW.obj_e0.isInitialized = 1;
22192 for (cartesian_trajectory_planner_B.i_mq = 0;
22193 cartesian_trajectory_planner_B.i_mq < 10;
22194 cartesian_trajectory_planner_B.i_mq++) {
22195 cartesian_trajectory_planner_B.cv2[cartesian_trajectory_planner_B.i_mq] =
22196 tmp_j[cartesian_trajectory_planner_B.i_mq];
22197 }
22198
22199 cartesian_trajectory_planner_B.cv2[10] = '\x00';
22200 ParamGet_cartesian_trajectory_planner_303.initialize
22201 (cartesian_trajectory_planner_B.cv2);
22202 ParamGet_cartesian_trajectory_planner_303.initialize_error_codes(0, 1, 2, 3);
22203 ParamGet_cartesian_trajectory_planner_303.set_initial_value(0.0);
22204 cartesian_trajectory_planner_DW.obj_e0.isSetupComplete = true;
22205
22206 // End of Start for MATLABSystem: '<S9>/Get Parameter2'
22207
22208 // Start for MATLABSystem: '<S9>/Get Parameter3'
22209 cartesian_trajectory_planner_DW.obj_a.matlabCodegenIsDeleted = false;
22210 cartesian_trajectory_planner_DW.obj_a.isInitialized = 1;
22211 for (cartesian_trajectory_planner_B.i_mq = 0;
22212 cartesian_trajectory_planner_B.i_mq < 10;
22213 cartesian_trajectory_planner_B.i_mq++) {
22214 cartesian_trajectory_planner_B.cv2[cartesian_trajectory_planner_B.i_mq] =
22215 tmp_k[cartesian_trajectory_planner_B.i_mq];
22216 }
22217
22218 cartesian_trajectory_planner_B.cv2[10] = '\x00';
22219 ParamGet_cartesian_trajectory_planner_304.initialize
22220 (cartesian_trajectory_planner_B.cv2);
22221 ParamGet_cartesian_trajectory_planner_304.initialize_error_codes(0, 1, 2, 3);
22222 ParamGet_cartesian_trajectory_planner_304.set_initial_value(1.0);
22223 cartesian_trajectory_planner_DW.obj_a.isSetupComplete = true;
22224
22225 // End of Start for MATLABSystem: '<S9>/Get Parameter3'
22226
22227 // Start for MATLABSystem: '<S9>/Get Parameter4'
22228 cartesian_trajectory_planner_DW.obj_p.matlabCodegenIsDeleted = false;
22229 cartesian_trajectory_planner_DW.obj_p.isInitialized = 1;
22230 for (cartesian_trajectory_planner_B.i_mq = 0;
22231 cartesian_trajectory_planner_B.i_mq < 10;
22232 cartesian_trajectory_planner_B.i_mq++) {
22233 cartesian_trajectory_planner_B.cv2[cartesian_trajectory_planner_B.i_mq] =
22234 tmp_l[cartesian_trajectory_planner_B.i_mq];
22235 }
22236
22237 cartesian_trajectory_planner_B.cv2[10] = '\x00';
22238 ParamGet_cartesian_trajectory_planner_305.initialize
22239 (cartesian_trajectory_planner_B.cv2);
22240 ParamGet_cartesian_trajectory_planner_305.initialize_error_codes(0, 1, 2, 3);
22241 ParamGet_cartesian_trajectory_planner_305.set_initial_value(1.0);
22242 cartesian_trajectory_planner_DW.obj_p.isSetupComplete = true;
22243
22244 // End of Start for MATLABSystem: '<S9>/Get Parameter4'
22245
22246 // Start for MATLABSystem: '<S9>/Get Parameter5'
22247 cartesian_trajectory_planner_DW.obj_l.matlabCodegenIsDeleted = false;
22248 cartesian_trajectory_planner_DW.obj_l.isInitialized = 1;
22249 for (cartesian_trajectory_planner_B.i_mq = 0;
22250 cartesian_trajectory_planner_B.i_mq < 10;
22251 cartesian_trajectory_planner_B.i_mq++) {
22252 cartesian_trajectory_planner_B.cv2[cartesian_trajectory_planner_B.i_mq] =
22253 tmp_m[cartesian_trajectory_planner_B.i_mq];
22254 }
22255
22256 cartesian_trajectory_planner_B.cv2[10] = '\x00';
22257 ParamGet_cartesian_trajectory_planner_306.initialize
22258 (cartesian_trajectory_planner_B.cv2);
22259 ParamGet_cartesian_trajectory_planner_306.initialize_error_codes(0, 1, 2, 3);
22260 ParamGet_cartesian_trajectory_planner_306.set_initial_value(1.0);
22261 cartesian_trajectory_planner_DW.obj_l.isSetupComplete = true;
22262
22263 // End of Start for MATLABSystem: '<S9>/Get Parameter5'
22264 emxInitStruct_robotics_slmanip_(&cartesian_trajectory_planner_DW.obj);
22265 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_1);
22266 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_50);
22267 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_49);
22268 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_48);
22269 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_47);
22270 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_46);
22271 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_45);
22272 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_44);
22273 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_43);
22274 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_42);
22275 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_41);
22276 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_40);
22277 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_39);
22278 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_38);
22279 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_37);
22280 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_36);
22281 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_35);
22282 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_34);
22283 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_33);
22284 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_32);
22285 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_31);
22286 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_30);
22287 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_29);
22288 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_28);
22289 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_27);
22290 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_26);
22291 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_25);
22292 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_24);
22293 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_23);
22294 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_22);
22295 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_21);
22296 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_20);
22297 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_19);
22298 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_18);
22299 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_17);
22300 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_16);
22301 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_15);
22302 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_14);
22303 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_13);
22304 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_12);
22305 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_11);
22306 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_10);
22307 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_9);
22308 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_8);
22309 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_7);
22310 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_6);
22311 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_5);
22312 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_4);
22313 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_3);
22314 emxInitStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_2);
22315 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_51);
22316 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_82);
22317 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_81);
22318 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_80);
22319 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_79);
22320 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_78);
22321 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_77);
22322 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_76);
22323 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_75);
22324 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_74);
22325 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_73);
22326 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_72);
22327 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_71);
22328 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_70);
22329 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_69);
22330 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_68);
22331 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_67);
22332 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_66);
22333 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_65);
22334 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_64);
22335 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_63);
22336 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_62);
22337 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_61);
22338 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_60);
22339 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_59);
22340 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_58);
22341 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_57);
22342 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_56);
22343 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_55);
22344 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_54);
22345 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_53);
22346 emxInitStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_52);
22347 emxInitStruct_x_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_83);
22348 emxInitStruct_x_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_84);
22349 emxInitStruct_f_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_85);
22350 emxInitStruct_h_robotics_core_i(&cartesian_trajectory_planner_DW.gobj_86);
22351 emxInitStruct_h_robotics_core_i(&cartesian_trajectory_planner_DW.gobj_87);
22352 emxInitStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_88);
22353 emxInitStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_103);
22354 emxInitStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_102);
22355 emxInitStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_101);
22356 emxInitStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_100);
22357 emxInitStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_99);
22358 emxInitStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_98);
22359 emxInitStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_97);
22360 emxInitStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_96);
22361 emxInitStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_95);
22362 emxInitStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_94);
22363 emxInitStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_93);
22364 emxInitStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_92);
22365 emxInitStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_91);
22366 emxInitStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_90);
22367 emxInitStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_89);
22368
22369 // Start for MATLABSystem: '<S3>/MATLAB System'
22370 cartesian_trajectory_planner_DW.obj.IKInternal.matlabCodegenIsDeleted = true;
22371 cartesian_trajectory_planner_DW.obj.matlabCodegenIsDeleted = true;
22372 cartesian_tr_eml_rand_mt19937ar(cartesian_trajectory_planner_DW.state_b);
22373 cartesian_trajectory_planner_DW.obj.isInitialized = 0;
22374 cartesian_trajectory_planner_DW.obj.matlabCodegenIsDeleted = false;
22375 obj = &cartesian_trajectory_planner_DW.obj;
22376 cartesian_trajectory_planner_DW.obj.isInitialized = 1;
22377 c_RigidBodyTree_RigidBodyTree_a
22378 (&cartesian_trajectory_planner_DW.obj.TreeInternal,
22379 &cartesian_trajectory_planner_DW.gobj_90,
22380 &cartesian_trajectory_planner_DW.gobj_91,
22381 &cartesian_trajectory_planner_DW.gobj_92,
22382 &cartesian_trajectory_planner_DW.gobj_93,
22383 &cartesian_trajectory_planner_DW.gobj_94,
22384 &cartesian_trajectory_planner_DW.gobj_95,
22385 &cartesian_trajectory_planner_DW.gobj_96,
22386 &cartesian_trajectory_planner_DW.gobj_89);
22387 obj_0 = &cartesian_trajectory_planner_DW.obj.IKInternal;
22388 obj_0->isInitialized = 0;
22389 inverseKinematics_set_RigidBody(obj_0, &obj->TreeInternal,
22390 &cartesian_trajectory_planner_DW.gobj_69,
22391 &cartesian_trajectory_planner_DW.gobj_70,
22392 &cartesian_trajectory_planner_DW.gobj_71,
22393 &cartesian_trajectory_planner_DW.gobj_72,
22394 &cartesian_trajectory_planner_DW.gobj_73,
22395 &cartesian_trajectory_planner_DW.gobj_74,
22396 &cartesian_trajectory_planner_DW.gobj_75,
22397 &cartesian_trajectory_planner_DW.gobj_76,
22398 &cartesian_trajectory_planner_DW.gobj_77,
22399 &cartesian_trajectory_planner_DW.gobj_78,
22400 &cartesian_trajectory_planner_DW.gobj_79,
22401 &cartesian_trajectory_planner_DW.gobj_80,
22402 &cartesian_trajectory_planner_DW.gobj_81,
22403 &cartesian_trajectory_planner_DW.gobj_82,
22404 &cartesian_trajectory_planner_DW.gobj_51,
22405 &cartesian_trajectory_planner_DW.gobj_28,
22406 &cartesian_trajectory_planner_DW.gobj_29,
22407 &cartesian_trajectory_planner_DW.gobj_30,
22408 &cartesian_trajectory_planner_DW.gobj_31,
22409 &cartesian_trajectory_planner_DW.gobj_32,
22410 &cartesian_trajectory_planner_DW.gobj_33,
22411 &cartesian_trajectory_planner_DW.gobj_34,
22412 &cartesian_trajectory_planner_DW.gobj_35,
22413 &cartesian_trajectory_planner_DW.gobj_36,
22414 &cartesian_trajectory_planner_DW.gobj_37,
22415 &cartesian_trajectory_planner_DW.gobj_38,
22416 &cartesian_trajectory_planner_DW.gobj_39,
22417 &cartesian_trajectory_planner_DW.gobj_40,
22418 &cartesian_trajectory_planner_DW.gobj_41,
22419 &cartesian_trajectory_planner_DW.gobj_42,
22420 &cartesian_trajectory_planner_DW.gobj_43,
22421 &cartesian_trajectory_planner_DW.gobj_44,
22422 &cartesian_trajectory_planner_DW.gobj_45,
22423 &cartesian_trajectory_planner_DW.gobj_46,
22424 &cartesian_trajectory_planner_DW.gobj_47,
22425 &cartesian_trajectory_planner_DW.gobj_48,
22426 &cartesian_trajectory_planner_DW.gobj_49,
22427 &cartesian_trajectory_planner_DW.gobj_50,
22428 &cartesian_trajectory_planner_DW.gobj_1,
22429 &cartesian_trajectory_planner_DW.gobj_27,
22430 &cartesian_trajectory_planner_DW.gobj_68,
22431 &cartesian_trajectory_planner_DW.gobj_83);
22432 obj_0->Solver = DampedBFGSwGradientProjection_D
22433 (&cartesian_trajectory_planner_DW.gobj_87);
22434 obj_1 = obj_0->Solver;
22435 obj_1->MaxNumIteration = 1500.0;
22436 obj_1->MaxTime = 10.0;
22437 obj_1->GradientTolerance = 1.0E-7;
22438 obj_1->SolutionTolerance = 1.0E-6;
22439 obj_1->ConstraintsOn = true;
22440 obj_1->RandomRestart = false;
22441 obj_1->StepTolerance = 1.0E-14;
22442 obj_0->matlabCodegenIsDeleted = false;
22443 emxInitStruct_robotics_slmani_a(&cartesian_trajectory_planner_DW.obj_o);
22444 emxInitStruct_n_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_1_n);
22445 emxInitStruct_n_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_16_d);
22446 emxInitStruct_n_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_15_h);
22447 emxInitStruct_n_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_14_i);
22448 emxInitStruct_n_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_13_m);
22449 emxInitStruct_n_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_12_j);
22450 emxInitStruct_n_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_11_h);
22451 emxInitStruct_n_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_10_b);
22452 emxInitStruct_n_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_9_a);
22453 emxInitStruct_n_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_8_h);
22454 emxInitStruct_n_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_7_a);
22455 emxInitStruct_n_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_6_m);
22456 emxInitStruct_n_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_5_c);
22457 emxInitStruct_n_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_4_p);
22458 emxInitStruct_n_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_3_m);
22459 emxInitStruct_n_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_2_d);
22460
22461 // Start for MATLABSystem: '<S2>/MATLAB System'
22462 cartesian_trajectory_planner_DW.obj_o.isInitialized = 0;
22463 cartesian_trajectory_planner_DW.obj_o.isInitialized = 1;
22464 car_RigidBodyTree_RigidBodyTree
22465 (&cartesian_trajectory_planner_DW.obj_o.TreeInternal,
22466 &cartesian_trajectory_planner_DW.gobj_2_d,
22467 &cartesian_trajectory_planner_DW.gobj_4_p,
22468 &cartesian_trajectory_planner_DW.gobj_5_c,
22469 &cartesian_trajectory_planner_DW.gobj_6_m,
22470 &cartesian_trajectory_planner_DW.gobj_7_a,
22471 &cartesian_trajectory_planner_DW.gobj_8_h,
22472 &cartesian_trajectory_planner_DW.gobj_9_a,
22473 &cartesian_trajectory_planner_DW.gobj_3_m);
22474 }
22475}
22476
22477// Model terminate function
22478void cartesian_trajectory_planner_terminate(void)
22479{
22480 // Terminate for Atomic SubSystem: '<Root>/Subscribe'
22481 // Terminate for MATLABSystem: '<S7>/SourceBlock'
22482 matlabCodegenHandle_matlabC_ast(&cartesian_trajectory_planner_DW.obj_j);
22483
22484 // End of Terminate for SubSystem: '<Root>/Subscribe'
22485
22486 // Terminate for MATLABSystem: '<S8>/Get Parameter'
22487 matlabCodegenHandle_matlab_astw(&cartesian_trajectory_planner_DW.obj_k);
22488
22489 // Terminate for MATLABSystem: '<S8>/Get Parameter1'
22490 matlabCodegenHandle_matlab_astw(&cartesian_trajectory_planner_DW.obj_ky);
22491
22492 // Terminate for MATLABSystem: '<S11>/Get Parameter3'
22493 matlabCodegenHandle_matlab_astw(&cartesian_trajectory_planner_DW.obj_g);
22494
22495 // Terminate for MATLABSystem: '<S11>/Get Parameter4'
22496 matlabCodegenHandle_matlab_astw(&cartesian_trajectory_planner_DW.obj_or);
22497
22498 // Terminate for MATLABSystem: '<S11>/Get Parameter5'
22499 matlabCodegenHandle_matlab_astw(&cartesian_trajectory_planner_DW.obj_dz);
22500
22501 // Terminate for MATLABSystem: '<S11>/Get Parameter6'
22502 matlabCodegenHandle_matlab_astw(&cartesian_trajectory_planner_DW.obj_di);
22503
22504 // Terminate for MATLABSystem: '<S11>/Get Parameter'
22505 matlabCodegenHandle_matlab_astw(&cartesian_trajectory_planner_DW.obj_b);
22506
22507 // Terminate for MATLABSystem: '<S11>/Get Parameter1'
22508 matlabCodegenHandle_matlab_astw(&cartesian_trajectory_planner_DW.obj_e0t);
22509
22510 // Terminate for MATLABSystem: '<S11>/Get Parameter2'
22511 matlabCodegenHandle_matlab_astw(&cartesian_trajectory_planner_DW.obj_pd);
22512
22513 // Terminate for MATLABSystem: '<S12>/Get Parameter3'
22514 matlabCodegenHandle_matlab_astw(&cartesian_trajectory_planner_DW.obj_f4);
22515
22516 // Terminate for MATLABSystem: '<S12>/Get Parameter4'
22517 matlabCodegenHandle_matlab_astw(&cartesian_trajectory_planner_DW.obj_en);
22518
22519 // Terminate for MATLABSystem: '<S12>/Get Parameter5'
22520 matlabCodegenHandle_matlab_astw(&cartesian_trajectory_planner_DW.obj_l0);
22521
22522 // Terminate for MATLABSystem: '<S12>/Get Parameter6'
22523 matlabCodegenHandle_matlab_astw(&cartesian_trajectory_planner_DW.obj_n);
22524
22525 // Terminate for MATLABSystem: '<S12>/Get Parameter'
22526 matlabCodegenHandle_matlab_astw(&cartesian_trajectory_planner_DW.obj_d);
22527
22528 // Terminate for MATLABSystem: '<S12>/Get Parameter1'
22529 matlabCodegenHandle_matlab_astw(&cartesian_trajectory_planner_DW.obj_oq);
22530
22531 // Terminate for MATLABSystem: '<S12>/Get Parameter2'
22532 matlabCodegenHandle_matlab_astw(&cartesian_trajectory_planner_DW.obj_es);
22533
22534 // Terminate for MATLABSystem: '<S9>/Get Parameter'
22535 matlabCodegenHandle_matlab_astw(&cartesian_trajectory_planner_DW.obj_c);
22536
22537 // Terminate for MATLABSystem: '<S9>/Get Parameter1'
22538 matlabCodegenHandle_matlab_astw(&cartesian_trajectory_planner_DW.obj_f);
22539
22540 // Terminate for MATLABSystem: '<S9>/Get Parameter2'
22541 matlabCodegenHandle_matlab_astw(&cartesian_trajectory_planner_DW.obj_e0);
22542
22543 // Terminate for MATLABSystem: '<S9>/Get Parameter3'
22544 matlabCodegenHandle_matlab_astw(&cartesian_trajectory_planner_DW.obj_a);
22545
22546 // Terminate for MATLABSystem: '<S9>/Get Parameter4'
22547 matlabCodegenHandle_matlab_astw(&cartesian_trajectory_planner_DW.obj_p);
22548
22549 // Terminate for MATLABSystem: '<S9>/Get Parameter5'
22550 matlabCodegenHandle_matlab_astw(&cartesian_trajectory_planner_DW.obj_l);
22551
22552 // Terminate for MATLABSystem: '<S3>/MATLAB System'
22553 matlabCodegenHandle_matlabCod_a(&cartesian_trajectory_planner_DW.obj);
22554 matlabCodegenHandle_matlabCodeg
22555 (&cartesian_trajectory_planner_DW.obj.IKInternal);
22556 emxFreeStruct_robotics_slmanip_(&cartesian_trajectory_planner_DW.obj);
22557 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_1);
22558 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_50);
22559 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_49);
22560 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_48);
22561 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_47);
22562 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_46);
22563 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_45);
22564 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_44);
22565 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_43);
22566 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_42);
22567 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_41);
22568 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_40);
22569 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_39);
22570 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_38);
22571 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_37);
22572 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_36);
22573 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_35);
22574 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_34);
22575 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_33);
22576 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_32);
22577 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_31);
22578 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_30);
22579 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_29);
22580 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_28);
22581 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_27);
22582 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_26);
22583 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_25);
22584 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_24);
22585 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_23);
22586 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_22);
22587 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_21);
22588 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_20);
22589 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_19);
22590 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_18);
22591 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_17);
22592 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_16);
22593 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_15);
22594 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_14);
22595 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_13);
22596 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_12);
22597 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_11);
22598 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_10);
22599 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_9);
22600 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_8);
22601 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_7);
22602 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_6);
22603 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_5);
22604 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_4);
22605 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_3);
22606 emxFreeStruct_c_rigidBodyJoint(&cartesian_trajectory_planner_DW.gobj_2);
22607 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_51);
22608 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_82);
22609 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_81);
22610 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_80);
22611 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_79);
22612 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_78);
22613 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_77);
22614 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_76);
22615 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_75);
22616 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_74);
22617 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_73);
22618 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_72);
22619 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_71);
22620 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_70);
22621 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_69);
22622 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_68);
22623 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_67);
22624 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_66);
22625 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_65);
22626 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_64);
22627 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_63);
22628 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_62);
22629 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_61);
22630 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_60);
22631 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_59);
22632 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_58);
22633 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_57);
22634 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_56);
22635 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_55);
22636 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_54);
22637 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_53);
22638 emxFreeStruct_w_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_52);
22639 emxFreeStruct_x_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_83);
22640 emxFreeStruct_x_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_84);
22641 emxFreeStruct_f_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_85);
22642 emxFreeStruct_h_robotics_core_i(&cartesian_trajectory_planner_DW.gobj_86);
22643 emxFreeStruct_h_robotics_core_i(&cartesian_trajectory_planner_DW.gobj_87);
22644 emxFreeStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_88);
22645 emxFreeStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_103);
22646 emxFreeStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_102);
22647 emxFreeStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_101);
22648 emxFreeStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_100);
22649 emxFreeStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_99);
22650 emxFreeStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_98);
22651 emxFreeStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_97);
22652 emxFreeStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_96);
22653 emxFreeStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_95);
22654 emxFreeStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_94);
22655 emxFreeStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_93);
22656 emxFreeStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_92);
22657 emxFreeStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_91);
22658 emxFreeStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_90);
22659 emxFreeStruct_v_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_89);
22660 emxFreeStruct_robotics_slmani_a(&cartesian_trajectory_planner_DW.obj_o);
22661 emxFreeStruct_n_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_1_n);
22662 emxFreeStruct_n_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_16_d);
22663 emxFreeStruct_n_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_15_h);
22664 emxFreeStruct_n_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_14_i);
22665 emxFreeStruct_n_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_13_m);
22666 emxFreeStruct_n_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_12_j);
22667 emxFreeStruct_n_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_11_h);
22668 emxFreeStruct_n_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_10_b);
22669 emxFreeStruct_n_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_9_a);
22670 emxFreeStruct_n_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_8_h);
22671 emxFreeStruct_n_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_7_a);
22672 emxFreeStruct_n_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_6_m);
22673 emxFreeStruct_n_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_5_c);
22674 emxFreeStruct_n_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_4_p);
22675 emxFreeStruct_n_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_3_m);
22676 emxFreeStruct_n_robotics_manip_(&cartesian_trajectory_planner_DW.gobj_2_d);
22677
22678 // Terminate for Atomic SubSystem: '<Root>/Publish'
22679 // Terminate for MATLABSystem: '<S6>/SinkBlock'
22680 matlabCodegenHandle_matlabCo_as(&cartesian_trajectory_planner_DW.obj_kh);
22681
22682 // End of Terminate for SubSystem: '<Root>/Publish'
22683}
22684
22685//
22686// File trailer for generated code.
22687//
22688// [EOF]
22689//
22690